1//
2// Academic License - for use in teaching, academic research, and meeting
3// course requirements at degree granting institutions only. Not for
4// government, commercial, or other organizational use.
5//
6// File: cartesian_trajectory_planner_2.cpp
7//
8// Code generated for Simulink model 'cartesian_trajectory_planner_2'.
9//
10// Model version : 1.129
11// Simulink Coder version : 9.3 (R2020a) 18-Nov-2019
12// C/C++ source code generated on : Tue May 19 21:41:37 2020
13//
14// Target selection: ert.tlc
15// Embedded hardware selection: Generic->Unspecified (assume 32-bit Generic)
16// Code generation objectives: Unspecified
17// Validation result: Not run
18//
19#include "cartesian_trajectory_planner_2.h"
20#include "cartesian_trajectory_planner_2_private.h"
21
22// Block signals (default storage)
23B_cartesian_trajectory_planne_T cartesian_trajectory_planner__B;
24
25// Block states (default storage)
26DW_cartesian_trajectory_plann_T cartesian_trajectory_planner_DW;
27
28// Real-time model
29RT_MODEL_cartesian_trajectory_T cartesian_trajectory_planner_M_ =
30 RT_MODEL_cartesian_trajectory_T();
31RT_MODEL_cartesian_trajectory_T *const cartesian_trajectory_planner_M =
32 &cartesian_trajectory_planner_M_;
33
34// Forward declaration for local functions
35static void cartesian_trajec_emxInit_char_T(emxArray_char_T_cartesian_tra_T
36 **pEmxArray, int32_T numDimensions, B_MATLABSystem_cartesian_traj_T *localB);
37static void cartesian_trajec_emxInit_real_T(emxArray_real_T_cartesian_tra_T
38 **pEmxArray, int32_T numDimensions, B_MATLABSystem_cartesian_traj_T *localB);
39static void emxInitStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_tr_T
40 *pStruct, B_MATLABSystem_cartesian_traj_T *localB);
41static void emxInitStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
42 *pStruct, B_MATLABSystem_cartesian_traj_T *localB);
43static void emxInitStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
44 *pStruct, B_MATLABSystem_cartesian_traj_T *localB);
45static void emxInitStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
46 *pStruct, B_MATLABSystem_cartesian_traj_T *localB);
47static void emxInitStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T
48 *pStruct, B_MATLABSystem_cartesian_traj_T *localB);
49static void emxInitStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
50 *pStruct, B_MATLABSystem_cartesian_traj_T *localB);
51static void emxInitStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
52 *pStruct, B_MATLABSystem_cartesian_traj_T *localB);
53static void emxInitStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
54 *pStruct, B_MATLABSystem_cartesian_traj_T *localB);
55static void emxInitStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
56 *pStruct, B_MATLABSystem_cartesian_traj_T *localB);
57static void cartesia_twister_state_vector_l(uint32_T mt[625]);
58static void cartesian_tr_eml_rand_mt19937ar(uint32_T state[625]);
59static void cartesian_traject_seed_to_state(uint32_T state[2]);
60static void cartesian_tra_eml_rand_shr3cong(uint32_T state[2]);
61static void cartes_emxEnsureCapacity_char_T(emxArray_char_T_cartesian_tra_T
62 *emxArray, int32_T oldNumel, B_MATLABSystem_cartesian_traj_T *localB);
63static void cartesian_trajec_emxFree_char_T(emxArray_char_T_cartesian_tra_T
64 **pEmxArray);
65static void cartes_emxEnsureCapacity_real_T(emxArray_real_T_cartesian_tra_T
66 *emxArray, int32_T oldNumel, B_MATLABSystem_cartesian_traj_T *localB);
67static v_robotics_manip_internal_Rig_T *cartesian_t_RigidBody_RigidBody
68 (v_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB);
69static v_robotics_manip_internal_Rig_T *cartesian_RigidBody_RigidBody_b
70 (v_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB);
71static v_robotics_manip_internal_Rig_T *cartesia_RigidBody_RigidBody_by
72 (v_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB);
73static v_robotics_manip_internal_Rig_T *cartesi_RigidBody_RigidBody_bya
74 (v_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB);
75static v_robotics_manip_internal_Rig_T *cartes_RigidBody_RigidBody_byap
76 (v_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB);
77static v_robotics_manip_internal_Rig_T *carte_RigidBody_RigidBody_byapf
78 (v_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB);
79static v_robotics_manip_internal_Rig_T *cart_RigidBody_RigidBody_byapfa
80 (v_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB);
81static y_robotics_manip_internal_Rig_T *car_RigidBodyTree_RigidBodyTree
82 (y_robotics_manip_internal_Rig_T *obj, v_robotics_manip_internal_Rig_T *iobj_0,
83 v_robotics_manip_internal_Rig_T *iobj_1, v_robotics_manip_internal_Rig_T
84 *iobj_2, v_robotics_manip_internal_Rig_T *iobj_3,
85 v_robotics_manip_internal_Rig_T *iobj_4, v_robotics_manip_internal_Rig_T
86 *iobj_5, v_robotics_manip_internal_Rig_T *iobj_6,
87 v_robotics_manip_internal_Rig_T *iobj_7, B_MATLABSystem_cartesian_traj_T
88 *localB);
89static void cartesi_genrand_uint32_vector_d(uint32_T mt[625], uint32_T u[2],
90 B_MATLABSystem_cartesian_traj_T *localB);
91static boolean_T cartesian_trajec_is_valid_state(const uint32_T mt[625],
92 B_MATLABSystem_cartesian_traj_T *localB);
93static void cartesian__eml_rand_mt19937ar_a(const uint32_T state[625], uint32_T
94 b_state[625], real_T *r, B_MATLABSystem_cartesian_traj_T *localB);
95static void cartesian_trajectory_plann_rand(real_T r[5],
96 B_MATLABSystem_cartesian_traj_T *localB, DW_MATLABSystem_cartesian_tra_T
97 *localDW);
98static w_robotics_manip_internal_Rig_T *car_RigidBody_RigidBody_byapfac
99 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_tr_T *iobj_0,
100 B_MATLABSystem_cartesian_traj_T *localB);
101static w_robotics_manip_internal_Rig_T *ca_RigidBody_RigidBody_byapfac3
102 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_tr_T *iobj_0,
103 B_MATLABSystem_cartesian_traj_T *localB);
104static w_robotics_manip_internal_Rig_T *c_RigidBody_RigidBody_byapfac3i
105 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_tr_T *iobj_0,
106 B_MATLABSystem_cartesian_traj_T *localB);
107static w_robotics_manip_internal_Rig_T *RigidBody_RigidBody_byapfac3iu
108 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_tr_T *iobj_0,
109 B_MATLABSystem_cartesian_traj_T *localB);
110static w_robotics_manip_internal_Rig_T *RigidBody_RigidBody_byapfac3iu2
111 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_tr_T *iobj_0,
112 B_MATLABSystem_cartesian_traj_T *localB);
113static void ca_RigidBodyTree_clearAllBodies(x_robotics_manip_internal_Rig_T *obj,
114 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
115 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
116 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
117 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
118 w_robotics_manip_internal_Rig_T *iobj_6, c_rigidBodyJoint_cartesian_tr_T
119 *iobj_7, c_rigidBodyJoint_cartesian_tr_T *iobj_8,
120 c_rigidBodyJoint_cartesian_tr_T *iobj_9, c_rigidBodyJoint_cartesian_tr_T
121 *iobj_10, c_rigidBodyJoint_cartesian_tr_T *iobj_11,
122 c_rigidBodyJoint_cartesian_tr_T *iobj_12, c_rigidBodyJoint_cartesian_tr_T
123 *iobj_13, c_rigidBodyJoint_cartesian_tr_T *iobj_14,
124 w_robotics_manip_internal_Rig_T *iobj_15, B_MATLABSystem_cartesian_traj_T
125 *localB, DW_MATLABSystem_cartesian_tra_T *localDW);
126static x_robotics_manip_internal_Rig_T *c_RigidBodyTree_RigidBodyTree_i
127 (x_robotics_manip_internal_Rig_T *obj, w_robotics_manip_internal_Rig_T *iobj_0,
128 w_robotics_manip_internal_Rig_T *iobj_1, w_robotics_manip_internal_Rig_T
129 *iobj_2, w_robotics_manip_internal_Rig_T *iobj_3,
130 w_robotics_manip_internal_Rig_T *iobj_4, w_robotics_manip_internal_Rig_T
131 *iobj_5, w_robotics_manip_internal_Rig_T *iobj_6,
132 c_rigidBodyJoint_cartesian_tr_T *iobj_7, c_rigidBodyJoint_cartesian_tr_T
133 *iobj_8, c_rigidBodyJoint_cartesian_tr_T *iobj_9,
134 c_rigidBodyJoint_cartesian_tr_T *iobj_10, c_rigidBodyJoint_cartesian_tr_T
135 *iobj_11, c_rigidBodyJoint_cartesian_tr_T *iobj_12,
136 c_rigidBodyJoint_cartesian_tr_T *iobj_13, c_rigidBodyJoint_cartesian_tr_T
137 *iobj_14, c_rigidBodyJoint_cartesian_tr_T *iobj_15,
138 w_robotics_manip_internal_Rig_T *iobj_16, B_MATLABSystem_cartesian_traj_T
139 *localB, DW_MATLABSystem_cartesian_tra_T *localDW);
140static boolean_T cartesian_trajectory_pla_strcmp(const
141 emxArray_char_T_cartesian_tra_T *a, const emxArray_char_T_cartesian_tra_T *b);
142static c_rigidBodyJoint_cartesian_tr_T *c_rigidBodyJoint_rigidBodyJoint
143 (c_rigidBodyJoint_cartesian_tr_T *obj, const emxArray_char_T_cartesian_tra_T
144 *jname, const emxArray_char_T_cartesian_tra_T *jtype,
145 B_MATLABSystem_cartesian_traj_T *localB);
146static void cartesian_trajec_emxFree_real_T(emxArray_real_T_cartesian_tra_T
147 **pEmxArray);
148static w_robotics_manip_internal_Rig_T *cartesian_trajec_RigidBody_copy(const
149 v_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_tr_T *iobj_0,
150 c_rigidBodyJoint_cartesian_tr_T *iobj_1, w_robotics_manip_internal_Rig_T
151 *iobj_2, B_MATLABSystem_cartesian_traj_T *localB);
152static void cartesian_RigidBodyTree_addBody(x_robotics_manip_internal_Rig_T *obj,
153 v_robotics_manip_internal_Rig_T *bodyin, const emxArray_char_T_cartesian_tra_T
154 *parentName, c_rigidBodyJoint_cartesian_tr_T *iobj_0,
155 c_rigidBodyJoint_cartesian_tr_T *iobj_1, w_robotics_manip_internal_Rig_T
156 *iobj_2, B_MATLABSystem_cartesian_traj_T *localB);
157static void inverseKinematics_set_RigidBody(b_inverseKinematics_cartesian_T *obj,
158 y_robotics_manip_internal_Rig_T *rigidbodytree,
159 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
160 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
161 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
162 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
163 w_robotics_manip_internal_Rig_T *iobj_6, w_robotics_manip_internal_Rig_T
164 *iobj_7, w_robotics_manip_internal_Rig_T *iobj_8,
165 w_robotics_manip_internal_Rig_T *iobj_9, w_robotics_manip_internal_Rig_T
166 *iobj_10, w_robotics_manip_internal_Rig_T *iobj_11,
167 w_robotics_manip_internal_Rig_T *iobj_12, w_robotics_manip_internal_Rig_T
168 *iobj_13, w_robotics_manip_internal_Rig_T *iobj_14,
169 c_rigidBodyJoint_cartesian_tr_T *iobj_15, c_rigidBodyJoint_cartesian_tr_T
170 *iobj_16, c_rigidBodyJoint_cartesian_tr_T *iobj_17,
171 c_rigidBodyJoint_cartesian_tr_T *iobj_18, c_rigidBodyJoint_cartesian_tr_T
172 *iobj_19, c_rigidBodyJoint_cartesian_tr_T *iobj_20,
173 c_rigidBodyJoint_cartesian_tr_T *iobj_21, c_rigidBodyJoint_cartesian_tr_T
174 *iobj_22, c_rigidBodyJoint_cartesian_tr_T *iobj_23,
175 c_rigidBodyJoint_cartesian_tr_T *iobj_24, c_rigidBodyJoint_cartesian_tr_T
176 *iobj_25, c_rigidBodyJoint_cartesian_tr_T *iobj_26,
177 c_rigidBodyJoint_cartesian_tr_T *iobj_27, c_rigidBodyJoint_cartesian_tr_T
178 *iobj_28, c_rigidBodyJoint_cartesian_tr_T *iobj_29,
179 c_rigidBodyJoint_cartesian_tr_T *iobj_30, c_rigidBodyJoint_cartesian_tr_T
180 *iobj_31, c_rigidBodyJoint_cartesian_tr_T *iobj_32,
181 c_rigidBodyJoint_cartesian_tr_T *iobj_33, c_rigidBodyJoint_cartesian_tr_T
182 *iobj_34, c_rigidBodyJoint_cartesian_tr_T *iobj_35,
183 c_rigidBodyJoint_cartesian_tr_T *iobj_36, c_rigidBodyJoint_cartesian_tr_T
184 *iobj_37, c_rigidBodyJoint_cartesian_tr_T *iobj_38,
185 c_rigidBodyJoint_cartesian_tr_T *iobj_39, w_robotics_manip_internal_Rig_T
186 *iobj_40, x_robotics_manip_internal_Rig_T *iobj_41,
187 B_MATLABSystem_cartesian_traj_T *localB, DW_MATLABSystem_cartesian_tra_T
188 *localDW);
189static h_robotics_core_internal_Damp_T *DampedBFGSwGradientProjection_D
190 (h_robotics_core_internal_Damp_T *obj);
191static void car_inverseKinematics_setupImpl(b_inverseKinematics_cartesian_T *obj,
192 f_robotics_manip_internal_IKE_T *iobj_0, B_MATLABSystem_cartesian_traj_T
193 *localB);
194static void c_inverseKinematics_setPoseGoal(b_inverseKinematics_cartesian_T *obj,
195 const real_T tform[16], const real_T weights[6],
196 B_MATLABSystem_cartesian_traj_T *localB);
197static void RigidBodyTree_validateConfigu_j(x_robotics_manip_internal_Rig_T *obj,
198 real_T Q[6], B_MATLABSystem_cartesian_traj_T *localB);
199static void ca_rigidBodyJoint_get_JointAxis(const
200 c_rigidBodyJoint_cartesian_tr_T *obj, real_T ax[3],
201 B_MATLABSystem_cartesian_traj_T *localB);
202static void cartesian_trajectory_planne_cat(real_T varargin_1, real_T varargin_2,
203 real_T varargin_3, real_T varargin_4, real_T varargin_5, real_T varargin_6,
204 real_T varargin_7, real_T varargin_8, real_T varargin_9, real_T y[9]);
205static void rigidBodyJoint_transformBodyToP(const
206 c_rigidBodyJoint_cartesian_tr_T *obj, real_T T[16],
207 B_MATLABSystem_cartesian_traj_T *localB);
208static void rigidBodyJoint_transformBodyT_n(const
209 c_rigidBodyJoint_cartesian_tr_T *obj, const real_T q_data[], const int32_T
210 *q_size, real_T T[16], B_MATLABSystem_cartesian_traj_T *localB);
211static void RigidBodyTree_efficientFKAndJac(x_robotics_manip_internal_Rig_T *obj,
212 const real_T qv[6], const emxArray_char_T_cartesian_tra_T *body1Name, real_T
213 T_data[], int32_T T_size[2], emxArray_real_T_cartesian_tra_T *Jac,
214 B_MATLABSystem_cartesian_traj_T *localB);
215static creal_T cartesian_trajectory_plann_sqrt(const creal_T x);
216static real_T cartesian_trajectory_plan_xnrm2(int32_T n, const real_T x[9],
217 int32_T ix0);
218static real_T cartesian_trajectory_pl_xnrm2_a(const real_T x[3], int32_T ix0);
219static real_T cartesian_trajectory_plan_xdotc(int32_T n, const real_T x[9],
220 int32_T ix0, const real_T y[9], int32_T iy0);
221static void cartesian_trajectory_plan_xaxpy(int32_T n, real_T a, int32_T ix0,
222 const real_T y[9], int32_T iy0, real_T b_y[9]);
223static void cartesian_trajectory__xaxpy_cxo(int32_T n, real_T a, const real_T x
224 [9], int32_T ix0, real_T y[3], int32_T iy0);
225static void cartesian_trajectory_p_xaxpy_cx(int32_T n, real_T a, const real_T x
226 [3], int32_T ix0, const real_T y[9], int32_T iy0, real_T b_y[9]);
227static void cartesian_trajectory_plan_xrotg(real_T a, real_T b, real_T *b_a,
228 real_T *b_b, real_T *c, real_T *s, B_MATLABSystem_cartesian_traj_T *localB);
229static void cartesian_trajectory_plann_xrot(const real_T x[9], int32_T ix0,
230 int32_T iy0, real_T c, real_T s, real_T b_x[9]);
231static void cartesian_trajectory_plan_xswap(const real_T x[9], int32_T ix0,
232 int32_T iy0, real_T b_x[9]);
233static void cartesian_trajectory_planne_svd(const real_T A[9], real_T U[9],
234 real_T s[3], real_T V[9], B_MATLABSystem_cartesian_traj_T *localB);
235static void cartesian_trajectory_rotm2axang(const real_T R[9], real_T axang[4],
236 B_MATLABSystem_cartesian_traj_T *localB);
237static void cartesian_IKHelpers_computeCost(const real_T x[6],
238 f_robotics_manip_internal_IKE_T *args, real_T *cost, real_T W[36],
239 emxArray_real_T_cartesian_tra_T *Jac, f_robotics_manip_internal_IKE_T **b_args,
240 B_MATLABSystem_cartesian_traj_T *localB);
241static void cartesian_trajectory_planne_eye(real_T b_I[36]);
242static void cartesian_tra_emxInit_boolean_T(emxArray_boolean_T_cartesian__T
243 **pEmxArray, int32_T numDimensions);
244static void cartesian_traje_emxInit_int32_T(emxArray_int32_T_cartesian_tr_T
245 **pEmxArray, int32_T numDimensions);
246static void car_emxEnsureCapacity_boolean_T(emxArray_boolean_T_cartesian__T
247 *emxArray, int32_T oldNumel);
248static void carte_emxEnsureCapacity_int32_T(emxArray_int32_T_cartesian_tr_T
249 *emxArray, int32_T oldNumel);
250static real_T SystemTimeProvider_getElapsedTi(const
251 f_robotics_core_internal_Syst_T *obj);
252static real_T cartesian_trajectory_pla_norm_j(const real_T x[6]);
253static void cartesian_tra_emxFree_boolean_T(emxArray_boolean_T_cartesian__T
254 **pEmxArray);
255static boolean_T DampedBFGSwGradientProjection_a(const
256 h_robotics_core_internal_Damp_T *obj, const real_T Hg[6], const
257 emxArray_real_T_cartesian_tra_T *alpha);
258static void cartesian_traje_emxFree_int32_T(emxArray_int32_T_cartesian_tr_T
259 **pEmxArray);
260static void cartesian_trajectory_pl_xzgetrf(int32_T m, int32_T n, const
261 emxArray_real_T_cartesian_tra_T *A, int32_T lda,
262 emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T *ipiv,
263 int32_T *info, B_MATLABSystem_cartesian_traj_T *localB);
264static void cartesian_trajectory_plan_xtrsm(int32_T m, int32_T n, const
265 emxArray_real_T_cartesian_tra_T *A, int32_T lda, const
266 emxArray_real_T_cartesian_tra_T *B, int32_T ldb,
267 emxArray_real_T_cartesian_tra_T *b_B, B_MATLABSystem_cartesian_traj_T *localB);
268static real_T cartesian_trajectory_p_xnrm2_ad(int32_T n, const
269 emxArray_real_T_cartesian_tra_T *x, int32_T ix0,
270 B_MATLABSystem_cartesian_traj_T *localB);
271static void cartesian_trajectory_pla_qrpf_b(const
272 emxArray_real_T_cartesian_tra_T *A, int32_T m, int32_T n,
273 emxArray_real_T_cartesian_tra_T *tau, const emxArray_int32_T_cartesian_tr_T
274 *jpvt, emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T
275 *b_jpvt, B_MATLABSystem_cartesian_traj_T *localB);
276static void cartesian_trajectory_p_mldivide(const
277 emxArray_real_T_cartesian_tra_T *A, const emxArray_real_T_cartesian_tra_T *B,
278 emxArray_real_T_cartesian_tra_T *Y, B_MATLABSystem_cartesian_traj_T *localB);
279static void cartesian_trajectory_planne_inv(const
280 emxArray_real_T_cartesian_tra_T *x, emxArray_real_T_cartesian_tra_T *y,
281 B_MATLABSystem_cartesian_traj_T *localB);
282static void cartesian_trajectory_plann_diag(const
283 emxArray_real_T_cartesian_tra_T *v, emxArray_real_T_cartesian_tra_T *d,
284 B_MATLABSystem_cartesian_traj_T *localB);
285static boolean_T cartesian_trajectory_planne_any(const
286 emxArray_boolean_T_cartesian__T *x);
287static void cartesian_trajectory_pl_sqrt_nh(emxArray_real_T_cartesian_tra_T *x);
288static boolean_T cartesian_tr_isPositiveDefinite(const real_T B[36],
289 B_MATLABSystem_cartesian_traj_T *localB);
290static boolean_T DampedBFGSwGradientProjection_k(const
291 h_robotics_core_internal_Damp_T *obj, const real_T xNew[6],
292 B_MATLABSystem_cartesian_traj_T *localB);
293static void DampedBFGSwGradientProjection_s(h_robotics_core_internal_Damp_T *obj,
294 real_T xSol[6], c_robotics_core_internal_NLPS_T *exitFlag, real_T *err, real_T
295 *iter, B_MATLABSystem_cartesian_traj_T *localB);
296static void cartesian_trajectory_p_isfinite(const
297 emxArray_real_T_cartesian_tra_T *x, emxArray_boolean_T_cartesian__T *b);
298static void cartesian_trajectory_pla_rand_m(real_T varargin_1,
299 emxArray_real_T_cartesian_tra_T *r, B_MATLABSystem_cartesian_traj_T *localB,
300 DW_MATLABSystem_cartesian_tra_T *localDW);
301static real_T cartesian_trajectory_genrandu_n(uint32_T mt[625],
302 B_MATLABSystem_cartesian_traj_T *localB);
303static real_T cartesia_eml_rand_mt19937ar_aoi(uint32_T state[625],
304 B_MATLABSystem_cartesian_traj_T *localB);
305static void cartesian_trajectory_plan_randn(const real_T varargin_1[2],
306 emxArray_real_T_cartesian_tra_T *r, B_MATLABSystem_cartesian_traj_T *localB,
307 DW_MATLABSystem_cartesian_tra_T *localDW);
308static void cartes_NLPSolverInterface_solve(h_robotics_core_internal_Damp_T *obj,
309 const real_T seed[6], real_T xSol[6], real_T *solutionInfo_Iterations, real_T *
310 solutionInfo_RRAttempts, real_T *solutionInfo_Error, real_T
311 *solutionInfo_ExitFlag, char_T solutionInfo_Status_data[], int32_T
312 solutionInfo_Status_size[2], B_MATLABSystem_cartesian_traj_T *localB,
313 DW_MATLABSystem_cartesian_tra_T *localDW);
314static void cart_inverseKinematics_stepImpl(b_inverseKinematics_cartesian_T *obj,
315 const real_T tform[16], const real_T weights[6], const real_T initialGuess[6],
316 real_T QSol[6], real_T *solutionInfo_Iterations, real_T
317 *solutionInfo_PoseErrorNorm, real_T *solutionInfo_ExitFlag, char_T
318 solutionInfo_Status_data[], int32_T solutionInfo_Status_size[2],
319 B_MATLABSystem_cartesian_traj_T *localB, DW_MATLABSystem_cartesian_tra_T
320 *localDW);
321static void matlabCodegenHandle_matlabCod_o(robotics_slmanip_internal_blo_T *obj);
322static void cartesian_tr_SystemCore_release(b_inverseKinematics_cartesian_T *obj);
323static void cartesian_tra_SystemCore_delete(b_inverseKinematics_cartesian_T *obj);
324static void matlabCodegenHandle_matlabCodeg(b_inverseKinematics_cartesian_T *obj);
325static void emxFreeStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_tr_T
326 *pStruct);
327static void emxFreeStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
328 *pStruct);
329static void emxFreeStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
330 *pStruct);
331static void emxFreeStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
332 *pStruct);
333static void emxFreeStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T
334 *pStruct);
335static void emxFreeStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
336 *pStruct);
337static void emxFreeStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
338 *pStruct);
339static void emxFreeStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
340 *pStruct);
341static void emxFreeStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
342 *pStruct);
343
344// Forward declaration for local functions
345static c_robotics_core_internal_code_T *NameValueParser_NameValueParser
346 (c_robotics_core_internal_code_T *obj);
347static void cartesian__computeProfileParams(real_T i, const real_T wayPoints[12],
348 const real_T Vel[6], const real_T Acc_data[], real_T *vParam, real_T *aParam,
349 real_T *tAParam, real_T *tFParam);
350static void c_computeScalarLSPBCoefficients(real_T s0, real_T sF, real_T v,
351 real_T a, real_T ta, real_T tf, real_T coefs[9], real_T breaks[4]);
352static boolean_T cart_checkPolyForMultipleBreaks(const real_T breakMat[24]);
353static void cartes_processPolynomialResults(const real_T breakMat[24], const
354 real_T coeffMat[54], boolean_T hasMultipleBreaks,
355 f_cell_wrap_cartesian_traject_T breaksCell[6], g_cell_wrap_cartesian_traject_T
356 coeffCell[6]);
357static real_T cartesian_trajector_rt_powd_snf(real_T u0, real_T u1);
358static void ca_addFlatSegmentsToPPFormParts(const real_T oldbreaks_data[], const
359 int32_T oldbreaks_size[2], const real_T oldCoeffs_data[], const int32_T
360 oldCoeffs_size[2], real_T dim, real_T newBreaks_data[], int32_T
361 newBreaks_size[2], real_T newCoefs_data[], int32_T newCoefs_size[2]);
362static void cartesian__polyCoeffsDerivative(const real_T coeffs_data[], const
363 int32_T coeffs_size[2], real_T dCoeffs_data[], int32_T dCoeffs_size[2]);
364static int32_T cartesian_trajectory_pl_bsearch(const real_T x_data[], const
365 int32_T x_size[2], real_T xi);
366static void cartesian_trajectory_plan_ppval(const real_T pp_breaks_data[], const
367 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], const int32_T
368 pp_coefs_size[3], const real_T x[2], real_T v_data[], int32_T v_size[2]);
369static void c_generateTrajectoriesFromCoefs(const real_T breaks_data[], const
370 int32_T breaks_size[2], const real_T coeffs_data[], const int32_T coeffs_size
371 [2], real_T dim, const real_T t[2], real_T q_data[], int32_T q_size[2], real_T
372 qd_data[], int32_T qd_size[2], real_T qdd_data[], int32_T qdd_size[2], real_T
373 pp_breaks_data[], int32_T pp_breaks_size[2], real_T pp_coefs_data[], int32_T
374 pp_coefs_size[3]);
375static void cartesian_trajector_trapveltraj(const real_T wayPoints[12], const
376 real_T varargin_2[6], real_T varargin_4, real_T q[12], real_T qd[12], real_T
377 qdd[12], real_T t[2], s_06c2DDfmr4zcnTqhww20ZC_cart_T ppCell_data[], int32_T
378 *ppCell_size);
379static void TrapVelTrajSys_extract1DimFromP(const real_T pp_breaks_data[], const
380 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], const int32_T
381 pp_coefs_size[3], real_T breaks_data[], int32_T breaks_size[2], real_T
382 oneDimCoeffs_data[], int32_T oneDimCoeffs_size[2]);
383static void cartesian_trajectory_pl_ppval_j(const real_T pp_breaks_data[], const
384 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], real_T x, real_T
385 v_data[], int32_T *v_size);
386static void TrapVelTrajSys_generate1DTrajec(const real_T breaks_data[], const
387 int32_T breaks_size[2], const real_T coefs_data[], const int32_T coefs_size[2],
388 real_T t, real_T q_data[], int32_T *q_size, real_T qd_data[], int32_T *qd_size,
389 real_T qdd_data[], int32_T *qdd_size);
390static void TrapVelTrajSys_extract1DimFro_j(const real_T pp_breaks_data[], const
391 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], const int32_T
392 pp_coefs_size[3], real_T breaks_data[], int32_T breaks_size[2], real_T
393 oneDimCoeffs_data[], int32_T oneDimCoeffs_size[2]);
394static void TrapVelTrajSys_extract1DimFr_jb(const real_T pp_breaks_data[], const
395 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], const int32_T
396 pp_coefs_size[3], real_T breaks_data[], int32_T breaks_size[2], real_T
397 oneDimCoeffs_data[], int32_T oneDimCoeffs_size[2]);
398static void TrapVelTrajSys_extract1DimF_jbz(const real_T pp_breaks_data[], const
399 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], const int32_T
400 pp_coefs_size[3], real_T breaks_data[], int32_T breaks_size[2], real_T
401 oneDimCoeffs_data[], int32_T oneDimCoeffs_size[2]);
402static void TrapVelTrajSys_extract1Dim_jbzp(const real_T pp_breaks_data[], const
403 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], const int32_T
404 pp_coefs_size[3], real_T breaks_data[], int32_T breaks_size[2], real_T
405 oneDimCoeffs_data[], int32_T oneDimCoeffs_size[2]);
406static void TrapVelTrajSys_extract1Di_jbzpa(const real_T pp_breaks_data[], const
407 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], const int32_T
408 pp_coefs_size[3], real_T breaks_data[], int32_T breaks_size[2], real_T
409 oneDimCoeffs_data[], int32_T oneDimCoeffs_size[2]);
410static void cartesi_TrapVelTrajSys_stepImpl(real_T time, const real_T
411 varargin_1[12], const real_T varargin_2[6], real_T varargin_3, real_T q[6],
412 real_T qd[6], real_T qdd[6]);
413static void matlabCodegenHandle_matlabC_jbz(ros_slros_internal_block_GetP_T *obj);
414static void matlabCodegenHandle_matlabCo_jb(ros_slros_internal_block_Subs_T *obj);
415static void matlabCodegenHandle_matlabCod_j(ros_slros_internal_block_Publ_T *obj);
416static void cartesian_traj_SystemCore_setup(robotics_slcore_internal_bl_j_T *obj);
417int32_T div_s32_floor(int32_T numerator, int32_T denominator)
418{
419 int32_T quotient;
420 uint32_T absNumerator;
421 uint32_T absDenominator;
422 uint32_T tempAbsQuotient;
423 boolean_T quotientNeedsNegation;
424 if (denominator == 0) {
425 quotient = numerator >= 0 ? MAX_int32_T : MIN_int32_T;
426
427 // Divide by zero handler
428 } else {
429 absNumerator = numerator < 0 ? ~static_cast<uint32_T>(numerator) + 1U :
430 static_cast<uint32_T>(numerator);
431 absDenominator = denominator < 0 ? ~static_cast<uint32_T>(denominator) + 1U :
432 static_cast<uint32_T>(denominator);
433 quotientNeedsNegation = ((numerator < 0) != (denominator < 0));
434 tempAbsQuotient = absNumerator / absDenominator;
435 if (quotientNeedsNegation) {
436 absNumerator %= absDenominator;
437 if (absNumerator > 0U) {
438 tempAbsQuotient++;
439 }
440 }
441
442 quotient = quotientNeedsNegation ? -static_cast<int32_T>(tempAbsQuotient) :
443 static_cast<int32_T>(tempAbsQuotient);
444 }
445
446 return quotient;
447}
448
449int32_T div_nzp_s32(int32_T numerator, int32_T denominator)
450{
451 uint32_T tempAbsQuotient;
452 tempAbsQuotient = (numerator < 0 ? ~static_cast<uint32_T>(numerator) + 1U :
453 static_cast<uint32_T>(numerator)) / (denominator < 0 ? ~
454 static_cast<uint32_T>(denominator) + 1U : static_cast<uint32_T>(denominator));
455 return (numerator < 0) != (denominator < 0) ? -static_cast<int32_T>
456 (tempAbsQuotient) : static_cast<int32_T>(tempAbsQuotient);
457}
458
459int32_T div_nzp_s32_floor(int32_T numerator, int32_T denominator)
460{
461 uint32_T absNumerator;
462 uint32_T absDenominator;
463 uint32_T tempAbsQuotient;
464 boolean_T quotientNeedsNegation;
465 absNumerator = numerator < 0 ? ~static_cast<uint32_T>(numerator) + 1U :
466 static_cast<uint32_T>(numerator);
467 absDenominator = denominator < 0 ? ~static_cast<uint32_T>(denominator) + 1U :
468 static_cast<uint32_T>(denominator);
469 quotientNeedsNegation = ((numerator < 0) != (denominator < 0));
470 tempAbsQuotient = absNumerator / absDenominator;
471 if (quotientNeedsNegation) {
472 absNumerator %= absDenominator;
473 if (absNumerator > 0U) {
474 tempAbsQuotient++;
475 }
476 }
477
478 return quotientNeedsNegation ? -static_cast<int32_T>(tempAbsQuotient) :
479 static_cast<int32_T>(tempAbsQuotient);
480}
481
482static void cartesian_trajec_emxInit_char_T(emxArray_char_T_cartesian_tra_T
483 **pEmxArray, int32_T numDimensions, B_MATLABSystem_cartesian_traj_T *localB)
484{
485 emxArray_char_T_cartesian_tra_T *emxArray;
486 *pEmxArray = (emxArray_char_T_cartesian_tra_T *)malloc(sizeof
487 (emxArray_char_T_cartesian_tra_T));
488 emxArray = *pEmxArray;
489 emxArray->data = (char_T *)NULL;
490 emxArray->numDimensions = numDimensions;
491 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
492 emxArray->allocatedSize = 0;
493 emxArray->canFreeData = true;
494 for (localB->i_b = 0; localB->i_b < numDimensions; localB->i_b++) {
495 emxArray->size[localB->i_b] = 0;
496 }
497}
498
499static void cartesian_trajec_emxInit_real_T(emxArray_real_T_cartesian_tra_T
500 **pEmxArray, int32_T numDimensions, B_MATLABSystem_cartesian_traj_T *localB)
501{
502 emxArray_real_T_cartesian_tra_T *emxArray;
503 *pEmxArray = (emxArray_real_T_cartesian_tra_T *)malloc(sizeof
504 (emxArray_real_T_cartesian_tra_T));
505 emxArray = *pEmxArray;
506 emxArray->data = (real_T *)NULL;
507 emxArray->numDimensions = numDimensions;
508 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
509 emxArray->allocatedSize = 0;
510 emxArray->canFreeData = true;
511 for (localB->i_m = 0; localB->i_m < numDimensions; localB->i_m++) {
512 emxArray->size[localB->i_m] = 0;
513 }
514}
515
516static void emxInitStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_tr_T
517 *pStruct, B_MATLABSystem_cartesian_traj_T *localB)
518{
519 cartesian_trajec_emxInit_char_T(&pStruct->Type, 2, localB);
520 cartesian_trajec_emxInit_real_T(&pStruct->MotionSubspace, 2, localB);
521 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2, localB);
522 cartesian_trajec_emxInit_real_T(&pStruct->PositionLimitsInternal, 2, localB);
523 cartesian_trajec_emxInit_real_T(&pStruct->HomePositionInternal, 1, localB);
524}
525
526static void emxInitStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
527 *pStruct, B_MATLABSystem_cartesian_traj_T *localB)
528{
529 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2, localB);
530 emxInitStruct_c_rigidBodyJoint(&pStruct->JointInternal, localB);
531}
532
533static void emxInitStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
534 *pStruct, B_MATLABSystem_cartesian_traj_T *localB)
535{
536 emxInitStruct_v_robotics_manip_(&pStruct->Base, localB);
537}
538
539static void emxInitStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
540 *pStruct, B_MATLABSystem_cartesian_traj_T *localB)
541{
542 cartesian_trajec_emxInit_real_T(&pStruct->Limits, 2, localB);
543}
544
545static void emxInitStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T
546 *pStruct, B_MATLABSystem_cartesian_traj_T *localB)
547{
548 emxInitStruct_y_robotics_manip_(&pStruct->TreeInternal, localB);
549 emxInitStruct_b_inverseKinemati(&pStruct->IKInternal, localB);
550}
551
552static void emxInitStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
553 *pStruct, B_MATLABSystem_cartesian_traj_T *localB)
554{
555 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2, localB);
556}
557
558static void emxInitStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
559 *pStruct, B_MATLABSystem_cartesian_traj_T *localB)
560{
561 emxInitStruct_w_robotics_manip_(&pStruct->Base, localB);
562}
563
564static void emxInitStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
565 *pStruct, B_MATLABSystem_cartesian_traj_T *localB)
566{
567 cartesian_trajec_emxInit_char_T(&pStruct->BodyName, 2, localB);
568 cartesian_trajec_emxInit_real_T(&pStruct->ErrTemp, 1, localB);
569 cartesian_trajec_emxInit_real_T(&pStruct->GradTemp, 1, localB);
570}
571
572static void emxInitStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
573 *pStruct, B_MATLABSystem_cartesian_traj_T *localB)
574{
575 cartesian_trajec_emxInit_real_T(&pStruct->ConstraintMatrix, 2, localB);
576 cartesian_trajec_emxInit_real_T(&pStruct->ConstraintBound, 1, localB);
577}
578
579static void cartesia_twister_state_vector_l(uint32_T mt[625])
580{
581 uint32_T r;
582 int32_T b_mti;
583 r = 5489U;
584 mt[0] = 5489U;
585 for (b_mti = 0; b_mti < 623; b_mti++) {
586 r = ((r >> 30U ^ r) * 1812433253U + b_mti) + 1U;
587 mt[b_mti + 1] = r;
588 }
589
590 mt[624] = 624U;
591}
592
593static void cartesian_tr_eml_rand_mt19937ar(uint32_T state[625])
594{
595 memset(&state[0], 0, 625U * sizeof(uint32_T));
596 cartesia_twister_state_vector_l(state);
597}
598
599static void cartesian_traject_seed_to_state(uint32_T state[2])
600{
601 state[0] = 362436069U;
602 state[1] = 521288629U;
603}
604
605static void cartesian_tra_eml_rand_shr3cong(uint32_T state[2])
606{
607 cartesian_traject_seed_to_state(state);
608}
609
610static void cartes_emxEnsureCapacity_char_T(emxArray_char_T_cartesian_tra_T
611 *emxArray, int32_T oldNumel, B_MATLABSystem_cartesian_traj_T *localB)
612{
613 void *newData;
614 if (oldNumel < 0) {
615 oldNumel = 0;
616 }
617
618 localB->newNumel = 1;
619 for (localB->i_fr = 0; localB->i_fr < emxArray->numDimensions; localB->i_fr++)
620 {
621 localB->newNumel *= emxArray->size[localB->i_fr];
622 }
623
624 if (localB->newNumel > emxArray->allocatedSize) {
625 localB->i_fr = emxArray->allocatedSize;
626 if (localB->i_fr < 16) {
627 localB->i_fr = 16;
628 }
629
630 while (localB->i_fr < localB->newNumel) {
631 if (localB->i_fr > 1073741823) {
632 localB->i_fr = MAX_int32_T;
633 } else {
634 localB->i_fr <<= 1;
635 }
636 }
637
638 newData = calloc(static_cast<uint32_T>(localB->i_fr), sizeof(char_T));
639 if (emxArray->data != NULL) {
640 memcpy(newData, emxArray->data, sizeof(char_T) * oldNumel);
641 if (emxArray->canFreeData) {
642 free(emxArray->data);
643 }
644 }
645
646 emxArray->data = (char_T *)newData;
647 emxArray->allocatedSize = localB->i_fr;
648 emxArray->canFreeData = true;
649 }
650}
651
652static void cartesian_trajec_emxFree_char_T(emxArray_char_T_cartesian_tra_T
653 **pEmxArray)
654{
655 if (*pEmxArray != (emxArray_char_T_cartesian_tra_T *)NULL) {
656 if (((*pEmxArray)->data != (char_T *)NULL) && (*pEmxArray)->canFreeData) {
657 free((*pEmxArray)->data);
658 }
659
660 free((*pEmxArray)->size);
661 free(*pEmxArray);
662 *pEmxArray = (emxArray_char_T_cartesian_tra_T *)NULL;
663 }
664}
665
666static void cartes_emxEnsureCapacity_real_T(emxArray_real_T_cartesian_tra_T
667 *emxArray, int32_T oldNumel, B_MATLABSystem_cartesian_traj_T *localB)
668{
669 void *newData;
670 if (oldNumel < 0) {
671 oldNumel = 0;
672 }
673
674 localB->newNumel_e = 1;
675 for (localB->i_nh = 0; localB->i_nh < emxArray->numDimensions; localB->i_nh++)
676 {
677 localB->newNumel_e *= emxArray->size[localB->i_nh];
678 }
679
680 if (localB->newNumel_e > emxArray->allocatedSize) {
681 localB->i_nh = emxArray->allocatedSize;
682 if (localB->i_nh < 16) {
683 localB->i_nh = 16;
684 }
685
686 while (localB->i_nh < localB->newNumel_e) {
687 if (localB->i_nh > 1073741823) {
688 localB->i_nh = MAX_int32_T;
689 } else {
690 localB->i_nh <<= 1;
691 }
692 }
693
694 newData = calloc(static_cast<uint32_T>(localB->i_nh), sizeof(real_T));
695 if (emxArray->data != NULL) {
696 memcpy(newData, emxArray->data, sizeof(real_T) * oldNumel);
697 if (emxArray->canFreeData) {
698 free(emxArray->data);
699 }
700 }
701
702 emxArray->data = (real_T *)newData;
703 emxArray->allocatedSize = localB->i_nh;
704 emxArray->canFreeData = true;
705 }
706}
707
708static v_robotics_manip_internal_Rig_T *cartesian_t_RigidBody_RigidBody
709 (v_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB)
710{
711 v_robotics_manip_internal_Rig_T *b_obj;
712 int8_T msubspace_data[36];
713 real_T poslim_data[12];
714 emxArray_char_T_cartesian_tra_T *switch_expression;
715 boolean_T b_bool;
716 int32_T b_kstr;
717 char_T b[8];
718 char_T b_0[9];
719 int32_T loop_ub;
720 int8_T tmp[6];
721 static const char_T tmp_0[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',
722 'l', 'i', 'n', 'k' };
723
724 static const real_T tmp_1[9] = { 0.012583419040406959, -0.00021487638648447484,
725 -0.00022605919127205462, -0.00021487638648447484, 0.00052369449451288713,
726 -0.00011525315957400814, -0.00022605919127205462, -0.00011525315957400814,
727 0.012646079447789898 };
728
729 static const real_T tmp_2[36] = { 0.012583419040406959,
730 -0.00021487638648447484, -0.00022605919127205462, 0.0, -0.00392971169381184,
731 0.00047022930128152475, -0.00021487638648447484, 0.00052369449451288713,
732 -0.00011525315957400814, 0.00392971169381184, 0.0, -0.00449464704691423,
733 -0.00022605919127205462, -0.00011525315957400814, 0.012646079447789898,
734 -0.00047022930128152475, 0.00449464704691423, 0.0, 0.0, 0.00392971169381184,
735 -0.00047022930128152475, 0.0785942338762368, 0.0, 0.0, -0.00392971169381184,
736 0.0, 0.00449464704691423, 0.0, 0.0785942338762368, 0.0,
737 0.00047022930128152475, -0.00449464704691423, 0.0, 0.0, 0.0,
738 0.0785942338762368 };
739
740 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
741 1 };
742
743 static const char_T tmp_4[15] = { 'w', 'o', 'r', 'l', 'd', '_', 'e', 'd', 'o',
744 '_', 'j', 'o', 'i', 'n', 't' };
745
746 static const char_T tmp_5[5] = { 'f', 'i', 'x', 'e', 'd' };
747
748 static const char_T tmp_6[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
749
750 static const char_T tmp_7[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
751
752 static const real_T tmp_8[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
753 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
754
755 static const real_T tmp_9[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
756 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
757
758 int32_T exitg1;
759 b_obj = obj;
760 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
761 obj->NameInternal->size[0] = 1;
762 obj->NameInternal->size[1] = 13;
763 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr, localB);
764 for (b_kstr = 0; b_kstr < 13; b_kstr++) {
765 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
766 }
767
768 obj->ParentIndex = 0.0;
769 obj->MassInternal = 0.0785942338762368;
770 obj->CenterOfMassInternal[0] = 0.057188;
771 obj->CenterOfMassInternal[1] = 0.005983;
772 obj->CenterOfMassInternal[2] = 0.05;
773 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
774 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
775 }
776
777 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
778 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
779 }
780
781 obj->JointInternal.InTree = false;
782 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
783 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
784 }
785
786 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
787 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
788 }
789
790 b_kstr = obj->JointInternal.NameInternal->size[0] *
791 obj->JointInternal.NameInternal->size[1];
792 obj->JointInternal.NameInternal->size[0] = 1;
793 obj->JointInternal.NameInternal->size[1] = 15;
794 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr,
795 localB);
796 for (b_kstr = 0; b_kstr < 15; b_kstr++) {
797 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
798 }
799
800 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
801 obj->JointInternal.Type->size[0] = 1;
802 obj->JointInternal.Type->size[1] = 5;
803 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr, localB);
804 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
805 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
806 }
807
808 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
809 b_kstr = switch_expression->size[0] * switch_expression->size[1];
810 switch_expression->size[0] = 1;
811 switch_expression->size[1] = obj->JointInternal.Type->size[1];
812 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr, localB);
813 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
814 - 1;
815 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
816 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
817 }
818
819 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
820 b[b_kstr] = tmp_6[b_kstr];
821 }
822
823 b_bool = false;
824 if (switch_expression->size[1] == 8) {
825 b_kstr = 1;
826 do {
827 exitg1 = 0;
828 if (b_kstr - 1 < 8) {
829 loop_ub = b_kstr - 1;
830 if (switch_expression->data[loop_ub] != b[loop_ub]) {
831 exitg1 = 1;
832 } else {
833 b_kstr++;
834 }
835 } else {
836 b_bool = true;
837 exitg1 = 1;
838 }
839 } while (exitg1 == 0);
840 }
841
842 if (b_bool) {
843 b_kstr = 0;
844 } else {
845 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
846 b_0[b_kstr] = tmp_7[b_kstr];
847 }
848
849 b_bool = false;
850 if (switch_expression->size[1] == 9) {
851 b_kstr = 1;
852 do {
853 exitg1 = 0;
854 if (b_kstr - 1 < 9) {
855 loop_ub = b_kstr - 1;
856 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
857 exitg1 = 1;
858 } else {
859 b_kstr++;
860 }
861 } else {
862 b_bool = true;
863 exitg1 = 1;
864 }
865 } while (exitg1 == 0);
866 }
867
868 if (b_bool) {
869 b_kstr = 1;
870 } else {
871 b_kstr = -1;
872 }
873 }
874
875 cartesian_trajec_emxFree_char_T(&switch_expression);
876 switch (b_kstr) {
877 case 0:
878 tmp[0] = 0;
879 tmp[1] = 0;
880 tmp[2] = 1;
881 tmp[3] = 0;
882 tmp[4] = 0;
883 tmp[5] = 0;
884 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
885 msubspace_data[b_kstr] = tmp[b_kstr];
886 }
887
888 poslim_data[0] = -3.1415926535897931;
889 poslim_data[1] = 3.1415926535897931;
890 obj->JointInternal.VelocityNumber = 1.0;
891 obj->JointInternal.PositionNumber = 1.0;
892 obj->JointInternal.JointAxisInternal[0] = 0.0;
893 obj->JointInternal.JointAxisInternal[1] = 0.0;
894 obj->JointInternal.JointAxisInternal[2] = 1.0;
895 break;
896
897 case 1:
898 tmp[0] = 0;
899 tmp[1] = 0;
900 tmp[2] = 0;
901 tmp[3] = 0;
902 tmp[4] = 0;
903 tmp[5] = 1;
904 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
905 msubspace_data[b_kstr] = tmp[b_kstr];
906 }
907
908 poslim_data[0] = -0.5;
909 poslim_data[1] = 0.5;
910 obj->JointInternal.VelocityNumber = 1.0;
911 obj->JointInternal.PositionNumber = 1.0;
912 obj->JointInternal.JointAxisInternal[0] = 0.0;
913 obj->JointInternal.JointAxisInternal[1] = 0.0;
914 obj->JointInternal.JointAxisInternal[2] = 1.0;
915 break;
916
917 default:
918 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
919 msubspace_data[b_kstr] = 0;
920 }
921
922 poslim_data[0] = 0.0;
923 poslim_data[1] = 0.0;
924 obj->JointInternal.VelocityNumber = 0.0;
925 obj->JointInternal.PositionNumber = 0.0;
926 obj->JointInternal.JointAxisInternal[0] = 0.0;
927 obj->JointInternal.JointAxisInternal[1] = 0.0;
928 obj->JointInternal.JointAxisInternal[2] = 0.0;
929 break;
930 }
931
932 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
933 obj->JointInternal.MotionSubspace->size[1];
934 obj->JointInternal.MotionSubspace->size[0] = 6;
935 obj->JointInternal.MotionSubspace->size[1] = 1;
936 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr,
937 localB);
938 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
939 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
940 }
941
942 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
943 obj->JointInternal.PositionLimitsInternal->size[1];
944 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
945 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
946 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
947 b_kstr, localB);
948 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
949 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
950 }
951
952 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
953 obj->JointInternal.HomePositionInternal->size[0] = 1;
954 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
955 b_kstr, localB);
956 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
957 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
958 }
959
960 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
961 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_8[b_kstr];
962 }
963
964 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
965 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_9[b_kstr];
966 }
967
968 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
969 obj->JointInternal.MotionSubspace->size[1];
970 obj->JointInternal.MotionSubspace->size[0] = 6;
971 obj->JointInternal.MotionSubspace->size[1] = 1;
972 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr,
973 localB);
974 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
975 obj->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
976 }
977
978 obj->JointInternal.InTree = true;
979 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
980 obj->JointInternal.PositionLimitsInternal->size[1];
981 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
982 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
983 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
984 b_kstr, localB);
985 obj->JointInternal.PositionLimitsInternal->data[0] = 0.0;
986 obj->JointInternal.PositionLimitsInternal->data
987 [obj->JointInternal.PositionLimitsInternal->size[0]] = 0.0;
988 obj->JointInternal.JointAxisInternal[0] = 0.0;
989 obj->JointInternal.JointAxisInternal[1] = 0.0;
990 obj->JointInternal.JointAxisInternal[2] = 0.0;
991 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
992 obj->JointInternal.HomePositionInternal->size[0] = 1;
993 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
994 b_kstr, localB);
995 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
996 return b_obj;
997}
998
999static v_robotics_manip_internal_Rig_T *cartesian_RigidBody_RigidBody_b
1000 (v_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB)
1001{
1002 v_robotics_manip_internal_Rig_T *b_obj;
1003 int8_T msubspace_data[36];
1004 real_T poslim_data[12];
1005 emxArray_char_T_cartesian_tra_T *switch_expression;
1006 boolean_T b_bool;
1007 int32_T b_kstr;
1008 char_T b[8];
1009 char_T b_0[9];
1010 int32_T loop_ub;
1011 int8_T tmp[6];
1012 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
1013 '1' };
1014
1015 static const real_T tmp_1[9] = { 0.012559660892485551, 0.00032713982710414,
1016 -1.32683892634271E-6, 0.00032713982710414, 0.00018048007331848145,
1017 9.17416945099368E-5, -1.32683892634271E-6, 9.17416945099368E-5,
1018 0.012672078048055372 };
1019
1020 static const real_T tmp_2[36] = { 0.012559660892485551, 0.00032713982710414,
1021 -1.32683892634271E-6, 0.0, -0.0, 0.0037143634929909515, 0.00032713982710414,
1022 0.00018048007331848145, 9.17416945099368E-5, 0.0, 0.0, 0.0029444543779393356,
1023 -1.32683892634271E-6, 9.17416945099368E-5, 0.012672078048055372,
1024 -0.0037143634929909515, -0.0029444543779393356, 0.0, 0.0, 0.0,
1025 -0.0037143634929909515, 0.0785942338762368, 0.0, 0.0, -0.0, 0.0,
1026 -0.0029444543779393356, 0.0, 0.0785942338762368, 0.0, 0.0037143634929909515,
1027 0.0029444543779393356, 0.0, 0.0, 0.0, 0.0785942338762368 };
1028
1029 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
1030 1 };
1031
1032 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
1033 '_', '1' };
1034
1035 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1036
1037 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
1038
1039 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
1040 0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };
1041
1042 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
1043 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
1044
1045 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1046 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1047 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
1048
1049 int32_T exitg1;
1050 b_obj = obj;
1051 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
1052 obj->NameInternal->size[0] = 1;
1053 obj->NameInternal->size[1] = 10;
1054 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr, localB);
1055 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
1056 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
1057 }
1058
1059 obj->ParentIndex = 1.0;
1060 obj->MassInternal = 0.0785942338762368;
1061 obj->CenterOfMassInternal[0] = -0.037464;
1062 obj->CenterOfMassInternal[1] = 0.04726;
1063 obj->CenterOfMassInternal[2] = 0.0;
1064 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
1065 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
1066 }
1067
1068 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
1069 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
1070 }
1071
1072 obj->JointInternal.InTree = false;
1073 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1074 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
1075 }
1076
1077 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1078 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
1079 }
1080
1081 b_kstr = obj->JointInternal.NameInternal->size[0] *
1082 obj->JointInternal.NameInternal->size[1];
1083 obj->JointInternal.NameInternal->size[0] = 1;
1084 obj->JointInternal.NameInternal->size[1] = 11;
1085 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr,
1086 localB);
1087 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
1088 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
1089 }
1090
1091 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
1092 obj->JointInternal.Type->size[0] = 1;
1093 obj->JointInternal.Type->size[1] = 8;
1094 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr, localB);
1095 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1096 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
1097 }
1098
1099 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
1100 b_kstr = switch_expression->size[0] * switch_expression->size[1];
1101 switch_expression->size[0] = 1;
1102 switch_expression->size[1] = obj->JointInternal.Type->size[1];
1103 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr, localB);
1104 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
1105 - 1;
1106 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1107 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
1108 }
1109
1110 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1111 b[b_kstr] = tmp_5[b_kstr];
1112 }
1113
1114 b_bool = false;
1115 if (switch_expression->size[1] == 8) {
1116 b_kstr = 1;
1117 do {
1118 exitg1 = 0;
1119 if (b_kstr - 1 < 8) {
1120 loop_ub = b_kstr - 1;
1121 if (switch_expression->data[loop_ub] != b[loop_ub]) {
1122 exitg1 = 1;
1123 } else {
1124 b_kstr++;
1125 }
1126 } else {
1127 b_bool = true;
1128 exitg1 = 1;
1129 }
1130 } while (exitg1 == 0);
1131 }
1132
1133 if (b_bool) {
1134 b_kstr = 0;
1135 } else {
1136 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
1137 b_0[b_kstr] = tmp_6[b_kstr];
1138 }
1139
1140 b_bool = false;
1141 if (switch_expression->size[1] == 9) {
1142 b_kstr = 1;
1143 do {
1144 exitg1 = 0;
1145 if (b_kstr - 1 < 9) {
1146 loop_ub = b_kstr - 1;
1147 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
1148 exitg1 = 1;
1149 } else {
1150 b_kstr++;
1151 }
1152 } else {
1153 b_bool = true;
1154 exitg1 = 1;
1155 }
1156 } while (exitg1 == 0);
1157 }
1158
1159 if (b_bool) {
1160 b_kstr = 1;
1161 } else {
1162 b_kstr = -1;
1163 }
1164 }
1165
1166 cartesian_trajec_emxFree_char_T(&switch_expression);
1167 switch (b_kstr) {
1168 case 0:
1169 tmp[0] = 0;
1170 tmp[1] = 0;
1171 tmp[2] = 1;
1172 tmp[3] = 0;
1173 tmp[4] = 0;
1174 tmp[5] = 0;
1175 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1176 msubspace_data[b_kstr] = tmp[b_kstr];
1177 }
1178
1179 poslim_data[0] = -3.1415926535897931;
1180 poslim_data[1] = 3.1415926535897931;
1181 obj->JointInternal.VelocityNumber = 1.0;
1182 obj->JointInternal.PositionNumber = 1.0;
1183 obj->JointInternal.JointAxisInternal[0] = 0.0;
1184 obj->JointInternal.JointAxisInternal[1] = 0.0;
1185 obj->JointInternal.JointAxisInternal[2] = 1.0;
1186 break;
1187
1188 case 1:
1189 tmp[0] = 0;
1190 tmp[1] = 0;
1191 tmp[2] = 0;
1192 tmp[3] = 0;
1193 tmp[4] = 0;
1194 tmp[5] = 1;
1195 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1196 msubspace_data[b_kstr] = tmp[b_kstr];
1197 }
1198
1199 poslim_data[0] = -0.5;
1200 poslim_data[1] = 0.5;
1201 obj->JointInternal.VelocityNumber = 1.0;
1202 obj->JointInternal.PositionNumber = 1.0;
1203 obj->JointInternal.JointAxisInternal[0] = 0.0;
1204 obj->JointInternal.JointAxisInternal[1] = 0.0;
1205 obj->JointInternal.JointAxisInternal[2] = 1.0;
1206 break;
1207
1208 default:
1209 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1210 msubspace_data[b_kstr] = 0;
1211 }
1212
1213 poslim_data[0] = 0.0;
1214 poslim_data[1] = 0.0;
1215 obj->JointInternal.VelocityNumber = 0.0;
1216 obj->JointInternal.PositionNumber = 0.0;
1217 obj->JointInternal.JointAxisInternal[0] = 0.0;
1218 obj->JointInternal.JointAxisInternal[1] = 0.0;
1219 obj->JointInternal.JointAxisInternal[2] = 0.0;
1220 break;
1221 }
1222
1223 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
1224 obj->JointInternal.MotionSubspace->size[1];
1225 obj->JointInternal.MotionSubspace->size[0] = 6;
1226 obj->JointInternal.MotionSubspace->size[1] = 1;
1227 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr,
1228 localB);
1229 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1230 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
1231 }
1232
1233 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
1234 obj->JointInternal.PositionLimitsInternal->size[1];
1235 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
1236 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
1237 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
1238 b_kstr, localB);
1239 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
1240 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
1241 }
1242
1243 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
1244 obj->JointInternal.HomePositionInternal->size[0] = 1;
1245 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
1246 b_kstr, localB);
1247 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
1248 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
1249 }
1250
1251 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1252 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
1253 }
1254
1255 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1256 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
1257 }
1258
1259 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
1260 obj->JointInternal.MotionSubspace->size[1];
1261 obj->JointInternal.MotionSubspace->size[0] = 6;
1262 obj->JointInternal.MotionSubspace->size[1] = 1;
1263 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr,
1264 localB);
1265 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1266 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
1267 }
1268
1269 obj->JointInternal.InTree = true;
1270 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
1271 obj->JointInternal.PositionLimitsInternal->size[1];
1272 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
1273 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
1274 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
1275 b_kstr, localB);
1276 obj->JointInternal.PositionLimitsInternal->data[0] = -3.10668606855;
1277 obj->JointInternal.PositionLimitsInternal->data
1278 [obj->JointInternal.PositionLimitsInternal->size[0]] = 3.10668606855;
1279 obj->JointInternal.JointAxisInternal[0] = 0.0;
1280 obj->JointInternal.JointAxisInternal[1] = 0.0;
1281 obj->JointInternal.JointAxisInternal[2] = 1.0;
1282 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
1283 obj->JointInternal.HomePositionInternal->size[0] = 1;
1284 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
1285 b_kstr, localB);
1286 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
1287 return b_obj;
1288}
1289
1290static v_robotics_manip_internal_Rig_T *cartesia_RigidBody_RigidBody_by
1291 (v_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB)
1292{
1293 v_robotics_manip_internal_Rig_T *b_obj;
1294 int8_T msubspace_data[36];
1295 real_T poslim_data[12];
1296 emxArray_char_T_cartesian_tra_T *switch_expression;
1297 boolean_T b_bool;
1298 int32_T b_kstr;
1299 char_T b[8];
1300 char_T b_0[9];
1301 int32_T loop_ub;
1302 int8_T tmp[6];
1303 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
1304 '2' };
1305
1306 static const real_T tmp_1[9] = { 0.012452446533447339, 0.00097164648860403489,
1307 -0.0012079400901817208, 0.00097164648860403489, 0.0077631932790618,
1308 0.0060529024261622953, -0.0012079400901817208, 0.0060529024261622953,
1309 0.0051581160550583753 };
1310
1311 static const real_T tmp_2[36] = { 0.012452446533447339, 0.00097164648860403489,
1312 -0.0012079400901817208, 0.0, 0.0036112478581453288, 0.0024995324199659588,
1313 0.00097164648860403489, 0.0077631932790618, 0.0060529024261622953,
1314 -0.0036112478581453288, 0.0, 0.0012907531029494371, -0.0012079400901817208,
1315 0.0060529024261622953, 0.0051581160550583753, -0.0024995324199659588,
1316 -0.0012907531029494371, 0.0, 0.0, -0.0036112478581453288,
1317 -0.0024995324199659588, 0.0785942338762368, 0.0, 0.0, 0.0036112478581453288,
1318 0.0, -0.0012907531029494371, 0.0, 0.0785942338762368, 0.0,
1319 0.0024995324199659588, 0.0012907531029494371, 0.0, 0.0, 0.0,
1320 0.0785942338762368 };
1321
1322 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
1323 1 };
1324
1325 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
1326 '_', '2' };
1327
1328 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1329
1330 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
1331
1332 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
1333 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
1334 0.0, 0.0, 0.0, 1.0 };
1335
1336 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
1337 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
1338
1339 static const real_T tmp_9[36] = { 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1340 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1341 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
1342
1343 int32_T exitg1;
1344 b_obj = obj;
1345 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
1346 obj->NameInternal->size[0] = 1;
1347 obj->NameInternal->size[1] = 10;
1348 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr, localB);
1349 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
1350 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
1351 }
1352
1353 obj->ParentIndex = 2.0;
1354 obj->MassInternal = 0.0785942338762368;
1355 obj->CenterOfMassInternal[0] = -0.016423;
1356 obj->CenterOfMassInternal[1] = 0.031803;
1357 obj->CenterOfMassInternal[2] = -0.045948;
1358 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
1359 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
1360 }
1361
1362 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
1363 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
1364 }
1365
1366 obj->JointInternal.InTree = false;
1367 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1368 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
1369 }
1370
1371 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1372 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
1373 }
1374
1375 b_kstr = obj->JointInternal.NameInternal->size[0] *
1376 obj->JointInternal.NameInternal->size[1];
1377 obj->JointInternal.NameInternal->size[0] = 1;
1378 obj->JointInternal.NameInternal->size[1] = 11;
1379 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr,
1380 localB);
1381 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
1382 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
1383 }
1384
1385 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
1386 obj->JointInternal.Type->size[0] = 1;
1387 obj->JointInternal.Type->size[1] = 8;
1388 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr, localB);
1389 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1390 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
1391 }
1392
1393 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
1394 b_kstr = switch_expression->size[0] * switch_expression->size[1];
1395 switch_expression->size[0] = 1;
1396 switch_expression->size[1] = obj->JointInternal.Type->size[1];
1397 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr, localB);
1398 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
1399 - 1;
1400 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1401 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
1402 }
1403
1404 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1405 b[b_kstr] = tmp_5[b_kstr];
1406 }
1407
1408 b_bool = false;
1409 if (switch_expression->size[1] == 8) {
1410 b_kstr = 1;
1411 do {
1412 exitg1 = 0;
1413 if (b_kstr - 1 < 8) {
1414 loop_ub = b_kstr - 1;
1415 if (switch_expression->data[loop_ub] != b[loop_ub]) {
1416 exitg1 = 1;
1417 } else {
1418 b_kstr++;
1419 }
1420 } else {
1421 b_bool = true;
1422 exitg1 = 1;
1423 }
1424 } while (exitg1 == 0);
1425 }
1426
1427 if (b_bool) {
1428 b_kstr = 0;
1429 } else {
1430 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
1431 b_0[b_kstr] = tmp_6[b_kstr];
1432 }
1433
1434 b_bool = false;
1435 if (switch_expression->size[1] == 9) {
1436 b_kstr = 1;
1437 do {
1438 exitg1 = 0;
1439 if (b_kstr - 1 < 9) {
1440 loop_ub = b_kstr - 1;
1441 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
1442 exitg1 = 1;
1443 } else {
1444 b_kstr++;
1445 }
1446 } else {
1447 b_bool = true;
1448 exitg1 = 1;
1449 }
1450 } while (exitg1 == 0);
1451 }
1452
1453 if (b_bool) {
1454 b_kstr = 1;
1455 } else {
1456 b_kstr = -1;
1457 }
1458 }
1459
1460 cartesian_trajec_emxFree_char_T(&switch_expression);
1461 switch (b_kstr) {
1462 case 0:
1463 tmp[0] = 0;
1464 tmp[1] = 0;
1465 tmp[2] = 1;
1466 tmp[3] = 0;
1467 tmp[4] = 0;
1468 tmp[5] = 0;
1469 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1470 msubspace_data[b_kstr] = tmp[b_kstr];
1471 }
1472
1473 poslim_data[0] = -3.1415926535897931;
1474 poslim_data[1] = 3.1415926535897931;
1475 obj->JointInternal.VelocityNumber = 1.0;
1476 obj->JointInternal.PositionNumber = 1.0;
1477 obj->JointInternal.JointAxisInternal[0] = 0.0;
1478 obj->JointInternal.JointAxisInternal[1] = 0.0;
1479 obj->JointInternal.JointAxisInternal[2] = 1.0;
1480 break;
1481
1482 case 1:
1483 tmp[0] = 0;
1484 tmp[1] = 0;
1485 tmp[2] = 0;
1486 tmp[3] = 0;
1487 tmp[4] = 0;
1488 tmp[5] = 1;
1489 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1490 msubspace_data[b_kstr] = tmp[b_kstr];
1491 }
1492
1493 poslim_data[0] = -0.5;
1494 poslim_data[1] = 0.5;
1495 obj->JointInternal.VelocityNumber = 1.0;
1496 obj->JointInternal.PositionNumber = 1.0;
1497 obj->JointInternal.JointAxisInternal[0] = 0.0;
1498 obj->JointInternal.JointAxisInternal[1] = 0.0;
1499 obj->JointInternal.JointAxisInternal[2] = 1.0;
1500 break;
1501
1502 default:
1503 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1504 msubspace_data[b_kstr] = 0;
1505 }
1506
1507 poslim_data[0] = 0.0;
1508 poslim_data[1] = 0.0;
1509 obj->JointInternal.VelocityNumber = 0.0;
1510 obj->JointInternal.PositionNumber = 0.0;
1511 obj->JointInternal.JointAxisInternal[0] = 0.0;
1512 obj->JointInternal.JointAxisInternal[1] = 0.0;
1513 obj->JointInternal.JointAxisInternal[2] = 0.0;
1514 break;
1515 }
1516
1517 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
1518 obj->JointInternal.MotionSubspace->size[1];
1519 obj->JointInternal.MotionSubspace->size[0] = 6;
1520 obj->JointInternal.MotionSubspace->size[1] = 1;
1521 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr,
1522 localB);
1523 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1524 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
1525 }
1526
1527 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
1528 obj->JointInternal.PositionLimitsInternal->size[1];
1529 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
1530 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
1531 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
1532 b_kstr, localB);
1533 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
1534 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
1535 }
1536
1537 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
1538 obj->JointInternal.HomePositionInternal->size[0] = 1;
1539 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
1540 b_kstr, localB);
1541 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
1542 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
1543 }
1544
1545 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1546 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
1547 }
1548
1549 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1550 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
1551 }
1552
1553 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
1554 obj->JointInternal.MotionSubspace->size[1];
1555 obj->JointInternal.MotionSubspace->size[0] = 6;
1556 obj->JointInternal.MotionSubspace->size[1] = 1;
1557 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr,
1558 localB);
1559 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1560 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
1561 }
1562
1563 obj->JointInternal.InTree = true;
1564 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
1565 obj->JointInternal.PositionLimitsInternal->size[1];
1566 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
1567 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
1568 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
1569 b_kstr, localB);
1570 obj->JointInternal.PositionLimitsInternal->data[0] = -1.71042266695;
1571 obj->JointInternal.PositionLimitsInternal->data
1572 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.71042266695;
1573 obj->JointInternal.JointAxisInternal[0] = 0.0;
1574 obj->JointInternal.JointAxisInternal[1] = 0.0;
1575 obj->JointInternal.JointAxisInternal[2] = -1.0;
1576 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
1577 obj->JointInternal.HomePositionInternal->size[0] = 1;
1578 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
1579 b_kstr, localB);
1580 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
1581 return b_obj;
1582}
1583
1584static v_robotics_manip_internal_Rig_T *cartesi_RigidBody_RigidBody_bya
1585 (v_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB)
1586{
1587 v_robotics_manip_internal_Rig_T *b_obj;
1588 int8_T msubspace_data[36];
1589 real_T poslim_data[12];
1590 emxArray_char_T_cartesian_tra_T *switch_expression;
1591 boolean_T b_bool;
1592 int32_T b_kstr;
1593 char_T b[8];
1594 char_T b_0[9];
1595 int32_T loop_ub;
1596 int8_T tmp[6];
1597 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
1598 '3' };
1599
1600 static const real_T tmp_1[9] = { 0.0123976159829631, 0.00022039108420015264,
1601 2.1332825710116445E-6, 0.00022039108420015264, 0.00014803877895089843,
1602 -9.2077339045129963E-5, 2.1332825710116445E-6, -9.2077339045129963E-5,
1603 0.012477575138803739 };
1604
1605 static const real_T tmp_2[36] = { 0.0123976159829631, 0.00022039108420015264,
1606 2.1332825710116445E-6, 0.0, -2.56217202436532E-5, 0.0010295844637787021,
1607 0.00022039108420015264, 0.00014803877895089843, -9.2077339045129963E-5,
1608 2.56217202436532E-5, 0.0, 0.0024737535112545539, 2.1332825710116445E-6,
1609 -9.2077339045129963E-5, 0.012477575138803739, -0.0010295844637787021,
1610 -0.0024737535112545539, 0.0, 0.0, 2.56217202436532E-5,
1611 -0.0010295844637787021, 0.0785942338762368, 0.0, 0.0, -2.56217202436532E-5,
1612 0.0, -0.0024737535112545539, 0.0, 0.0785942338762368, 0.0,
1613 0.0010295844637787021, 0.0024737535112545539, 0.0, 0.0, 0.0,
1614 0.0785942338762368 };
1615
1616 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
1617 1 };
1618
1619 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
1620 '_', '3' };
1621
1622 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1623
1624 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
1625
1626 static const real_T tmp_7[16] = { 1.0, 2.0682310711021444E-13,
1627 2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,
1628 2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,
1629 1.0 };
1630
1631 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
1632 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
1633
1634 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1635 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1636 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
1637
1638 int32_T exitg1;
1639 b_obj = obj;
1640 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
1641 obj->NameInternal->size[0] = 1;
1642 obj->NameInternal->size[1] = 10;
1643 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr, localB);
1644 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
1645 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
1646 }
1647
1648 obj->ParentIndex = 3.0;
1649 obj->MassInternal = 0.0785942338762368;
1650 obj->CenterOfMassInternal[0] = -0.031475;
1651 obj->CenterOfMassInternal[1] = 0.0131;
1652 obj->CenterOfMassInternal[2] = 0.000326;
1653 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
1654 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
1655 }
1656
1657 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
1658 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
1659 }
1660
1661 obj->JointInternal.InTree = false;
1662 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1663 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
1664 }
1665
1666 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1667 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
1668 }
1669
1670 b_kstr = obj->JointInternal.NameInternal->size[0] *
1671 obj->JointInternal.NameInternal->size[1];
1672 obj->JointInternal.NameInternal->size[0] = 1;
1673 obj->JointInternal.NameInternal->size[1] = 11;
1674 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr,
1675 localB);
1676 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
1677 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
1678 }
1679
1680 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
1681 obj->JointInternal.Type->size[0] = 1;
1682 obj->JointInternal.Type->size[1] = 8;
1683 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr, localB);
1684 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1685 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
1686 }
1687
1688 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
1689 b_kstr = switch_expression->size[0] * switch_expression->size[1];
1690 switch_expression->size[0] = 1;
1691 switch_expression->size[1] = obj->JointInternal.Type->size[1];
1692 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr, localB);
1693 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
1694 - 1;
1695 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1696 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
1697 }
1698
1699 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1700 b[b_kstr] = tmp_5[b_kstr];
1701 }
1702
1703 b_bool = false;
1704 if (switch_expression->size[1] == 8) {
1705 b_kstr = 1;
1706 do {
1707 exitg1 = 0;
1708 if (b_kstr - 1 < 8) {
1709 loop_ub = b_kstr - 1;
1710 if (switch_expression->data[loop_ub] != b[loop_ub]) {
1711 exitg1 = 1;
1712 } else {
1713 b_kstr++;
1714 }
1715 } else {
1716 b_bool = true;
1717 exitg1 = 1;
1718 }
1719 } while (exitg1 == 0);
1720 }
1721
1722 if (b_bool) {
1723 b_kstr = 0;
1724 } else {
1725 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
1726 b_0[b_kstr] = tmp_6[b_kstr];
1727 }
1728
1729 b_bool = false;
1730 if (switch_expression->size[1] == 9) {
1731 b_kstr = 1;
1732 do {
1733 exitg1 = 0;
1734 if (b_kstr - 1 < 9) {
1735 loop_ub = b_kstr - 1;
1736 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
1737 exitg1 = 1;
1738 } else {
1739 b_kstr++;
1740 }
1741 } else {
1742 b_bool = true;
1743 exitg1 = 1;
1744 }
1745 } while (exitg1 == 0);
1746 }
1747
1748 if (b_bool) {
1749 b_kstr = 1;
1750 } else {
1751 b_kstr = -1;
1752 }
1753 }
1754
1755 cartesian_trajec_emxFree_char_T(&switch_expression);
1756 switch (b_kstr) {
1757 case 0:
1758 tmp[0] = 0;
1759 tmp[1] = 0;
1760 tmp[2] = 1;
1761 tmp[3] = 0;
1762 tmp[4] = 0;
1763 tmp[5] = 0;
1764 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1765 msubspace_data[b_kstr] = tmp[b_kstr];
1766 }
1767
1768 poslim_data[0] = -3.1415926535897931;
1769 poslim_data[1] = 3.1415926535897931;
1770 obj->JointInternal.VelocityNumber = 1.0;
1771 obj->JointInternal.PositionNumber = 1.0;
1772 obj->JointInternal.JointAxisInternal[0] = 0.0;
1773 obj->JointInternal.JointAxisInternal[1] = 0.0;
1774 obj->JointInternal.JointAxisInternal[2] = 1.0;
1775 break;
1776
1777 case 1:
1778 tmp[0] = 0;
1779 tmp[1] = 0;
1780 tmp[2] = 0;
1781 tmp[3] = 0;
1782 tmp[4] = 0;
1783 tmp[5] = 1;
1784 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1785 msubspace_data[b_kstr] = tmp[b_kstr];
1786 }
1787
1788 poslim_data[0] = -0.5;
1789 poslim_data[1] = 0.5;
1790 obj->JointInternal.VelocityNumber = 1.0;
1791 obj->JointInternal.PositionNumber = 1.0;
1792 obj->JointInternal.JointAxisInternal[0] = 0.0;
1793 obj->JointInternal.JointAxisInternal[1] = 0.0;
1794 obj->JointInternal.JointAxisInternal[2] = 1.0;
1795 break;
1796
1797 default:
1798 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1799 msubspace_data[b_kstr] = 0;
1800 }
1801
1802 poslim_data[0] = 0.0;
1803 poslim_data[1] = 0.0;
1804 obj->JointInternal.VelocityNumber = 0.0;
1805 obj->JointInternal.PositionNumber = 0.0;
1806 obj->JointInternal.JointAxisInternal[0] = 0.0;
1807 obj->JointInternal.JointAxisInternal[1] = 0.0;
1808 obj->JointInternal.JointAxisInternal[2] = 0.0;
1809 break;
1810 }
1811
1812 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
1813 obj->JointInternal.MotionSubspace->size[1];
1814 obj->JointInternal.MotionSubspace->size[0] = 6;
1815 obj->JointInternal.MotionSubspace->size[1] = 1;
1816 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr,
1817 localB);
1818 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1819 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
1820 }
1821
1822 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
1823 obj->JointInternal.PositionLimitsInternal->size[1];
1824 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
1825 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
1826 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
1827 b_kstr, localB);
1828 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
1829 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
1830 }
1831
1832 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
1833 obj->JointInternal.HomePositionInternal->size[0] = 1;
1834 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
1835 b_kstr, localB);
1836 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
1837 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
1838 }
1839
1840 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1841 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
1842 }
1843
1844 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1845 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
1846 }
1847
1848 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
1849 obj->JointInternal.MotionSubspace->size[1];
1850 obj->JointInternal.MotionSubspace->size[0] = 6;
1851 obj->JointInternal.MotionSubspace->size[1] = 1;
1852 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr,
1853 localB);
1854 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1855 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
1856 }
1857
1858 obj->JointInternal.InTree = true;
1859 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
1860 obj->JointInternal.PositionLimitsInternal->size[1];
1861 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
1862 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
1863 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
1864 b_kstr, localB);
1865 obj->JointInternal.PositionLimitsInternal->data[0] = -1.71042266695;
1866 obj->JointInternal.PositionLimitsInternal->data
1867 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.71042266695;
1868 obj->JointInternal.JointAxisInternal[0] = 0.0;
1869 obj->JointInternal.JointAxisInternal[1] = 0.0;
1870 obj->JointInternal.JointAxisInternal[2] = 1.0;
1871 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
1872 obj->JointInternal.HomePositionInternal->size[0] = 1;
1873 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
1874 b_kstr, localB);
1875 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
1876 return b_obj;
1877}
1878
1879static v_robotics_manip_internal_Rig_T *cartes_RigidBody_RigidBody_byap
1880 (v_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB)
1881{
1882 v_robotics_manip_internal_Rig_T *b_obj;
1883 int8_T msubspace_data[36];
1884 real_T poslim_data[12];
1885 emxArray_char_T_cartesian_tra_T *switch_expression;
1886 boolean_T b_bool;
1887 int32_T b_kstr;
1888 char_T b[8];
1889 char_T b_0[9];
1890 int32_T loop_ub;
1891 int8_T tmp[6];
1892 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
1893 '4' };
1894
1895 static const real_T tmp_1[9] = { 0.0123990349928174, -2.7766271471639167E-6,
1896 0.00022466935228286869, -2.7766271471639167E-6, 0.012491487094789458,
1897 9.2330220708293281E-5, 0.00022466935228286869, 9.2330220708293281E-5,
1898 0.00016056153744711284 };
1899
1900 static const real_T tmp_2[36] = { 0.0123990349928174, -2.7766271471639167E-6,
1901 0.00022466935228286869, 0.0, 0.0010818496293063995, 4.275526322867282E-5,
1902 -2.7766271471639167E-6, 0.012491487094789458, 9.2330220708293281E-5,
1903 -0.0010818496293063995, 0.0, -0.0026650518765093138, 0.00022466935228286869,
1904 9.2330220708293281E-5, 0.00016056153744711284, -4.275526322867282E-5,
1905 0.0026650518765093138, 0.0, 0.0, -0.0010818496293063995,
1906 -4.275526322867282E-5, 0.0785942338762368, 0.0, 0.0, 0.0010818496293063995,
1907 0.0, 0.0026650518765093138, 0.0, 0.0785942338762368, 0.0,
1908 4.275526322867282E-5, -0.0026650518765093138, 0.0, 0.0, 0.0,
1909 0.0785942338762368 };
1910
1911 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
1912 1 };
1913
1914 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
1915 '_', '4' };
1916
1917 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1918
1919 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
1920
1921 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
1922 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
1923 0.0, -0.268, 0.0, 1.0 };
1924
1925 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
1926 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
1927
1928 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1929 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1930 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
1931
1932 int32_T exitg1;
1933 b_obj = obj;
1934 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
1935 obj->NameInternal->size[0] = 1;
1936 obj->NameInternal->size[1] = 10;
1937 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr, localB);
1938 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
1939 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
1940 }
1941
1942 obj->ParentIndex = 4.0;
1943 obj->MassInternal = 0.0785942338762368;
1944 obj->CenterOfMassInternal[0] = 0.033909;
1945 obj->CenterOfMassInternal[1] = 0.000544;
1946 obj->CenterOfMassInternal[2] = -0.013765;
1947 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
1948 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
1949 }
1950
1951 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
1952 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
1953 }
1954
1955 obj->JointInternal.InTree = false;
1956 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1957 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
1958 }
1959
1960 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1961 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
1962 }
1963
1964 b_kstr = obj->JointInternal.NameInternal->size[0] *
1965 obj->JointInternal.NameInternal->size[1];
1966 obj->JointInternal.NameInternal->size[0] = 1;
1967 obj->JointInternal.NameInternal->size[1] = 11;
1968 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr,
1969 localB);
1970 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
1971 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
1972 }
1973
1974 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
1975 obj->JointInternal.Type->size[0] = 1;
1976 obj->JointInternal.Type->size[1] = 8;
1977 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr, localB);
1978 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1979 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
1980 }
1981
1982 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
1983 b_kstr = switch_expression->size[0] * switch_expression->size[1];
1984 switch_expression->size[0] = 1;
1985 switch_expression->size[1] = obj->JointInternal.Type->size[1];
1986 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr, localB);
1987 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
1988 - 1;
1989 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1990 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
1991 }
1992
1993 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1994 b[b_kstr] = tmp_5[b_kstr];
1995 }
1996
1997 b_bool = false;
1998 if (switch_expression->size[1] == 8) {
1999 b_kstr = 1;
2000 do {
2001 exitg1 = 0;
2002 if (b_kstr - 1 < 8) {
2003 loop_ub = b_kstr - 1;
2004 if (switch_expression->data[loop_ub] != b[loop_ub]) {
2005 exitg1 = 1;
2006 } else {
2007 b_kstr++;
2008 }
2009 } else {
2010 b_bool = true;
2011 exitg1 = 1;
2012 }
2013 } while (exitg1 == 0);
2014 }
2015
2016 if (b_bool) {
2017 b_kstr = 0;
2018 } else {
2019 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
2020 b_0[b_kstr] = tmp_6[b_kstr];
2021 }
2022
2023 b_bool = false;
2024 if (switch_expression->size[1] == 9) {
2025 b_kstr = 1;
2026 do {
2027 exitg1 = 0;
2028 if (b_kstr - 1 < 9) {
2029 loop_ub = b_kstr - 1;
2030 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
2031 exitg1 = 1;
2032 } else {
2033 b_kstr++;
2034 }
2035 } else {
2036 b_bool = true;
2037 exitg1 = 1;
2038 }
2039 } while (exitg1 == 0);
2040 }
2041
2042 if (b_bool) {
2043 b_kstr = 1;
2044 } else {
2045 b_kstr = -1;
2046 }
2047 }
2048
2049 cartesian_trajec_emxFree_char_T(&switch_expression);
2050 switch (b_kstr) {
2051 case 0:
2052 tmp[0] = 0;
2053 tmp[1] = 0;
2054 tmp[2] = 1;
2055 tmp[3] = 0;
2056 tmp[4] = 0;
2057 tmp[5] = 0;
2058 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2059 msubspace_data[b_kstr] = tmp[b_kstr];
2060 }
2061
2062 poslim_data[0] = -3.1415926535897931;
2063 poslim_data[1] = 3.1415926535897931;
2064 obj->JointInternal.VelocityNumber = 1.0;
2065 obj->JointInternal.PositionNumber = 1.0;
2066 obj->JointInternal.JointAxisInternal[0] = 0.0;
2067 obj->JointInternal.JointAxisInternal[1] = 0.0;
2068 obj->JointInternal.JointAxisInternal[2] = 1.0;
2069 break;
2070
2071 case 1:
2072 tmp[0] = 0;
2073 tmp[1] = 0;
2074 tmp[2] = 0;
2075 tmp[3] = 0;
2076 tmp[4] = 0;
2077 tmp[5] = 1;
2078 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2079 msubspace_data[b_kstr] = tmp[b_kstr];
2080 }
2081
2082 poslim_data[0] = -0.5;
2083 poslim_data[1] = 0.5;
2084 obj->JointInternal.VelocityNumber = 1.0;
2085 obj->JointInternal.PositionNumber = 1.0;
2086 obj->JointInternal.JointAxisInternal[0] = 0.0;
2087 obj->JointInternal.JointAxisInternal[1] = 0.0;
2088 obj->JointInternal.JointAxisInternal[2] = 1.0;
2089 break;
2090
2091 default:
2092 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2093 msubspace_data[b_kstr] = 0;
2094 }
2095
2096 poslim_data[0] = 0.0;
2097 poslim_data[1] = 0.0;
2098 obj->JointInternal.VelocityNumber = 0.0;
2099 obj->JointInternal.PositionNumber = 0.0;
2100 obj->JointInternal.JointAxisInternal[0] = 0.0;
2101 obj->JointInternal.JointAxisInternal[1] = 0.0;
2102 obj->JointInternal.JointAxisInternal[2] = 0.0;
2103 break;
2104 }
2105
2106 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
2107 obj->JointInternal.MotionSubspace->size[1];
2108 obj->JointInternal.MotionSubspace->size[0] = 6;
2109 obj->JointInternal.MotionSubspace->size[1] = 1;
2110 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr,
2111 localB);
2112 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2113 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
2114 }
2115
2116 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
2117 obj->JointInternal.PositionLimitsInternal->size[1];
2118 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
2119 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
2120 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
2121 b_kstr, localB);
2122 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
2123 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
2124 }
2125
2126 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
2127 obj->JointInternal.HomePositionInternal->size[0] = 1;
2128 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
2129 b_kstr, localB);
2130 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
2131 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
2132 }
2133
2134 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
2135 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
2136 }
2137
2138 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
2139 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
2140 }
2141
2142 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
2143 obj->JointInternal.MotionSubspace->size[1];
2144 obj->JointInternal.MotionSubspace->size[0] = 6;
2145 obj->JointInternal.MotionSubspace->size[1] = 1;
2146 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr,
2147 localB);
2148 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2149 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
2150 }
2151
2152 obj->JointInternal.InTree = true;
2153 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
2154 obj->JointInternal.PositionLimitsInternal->size[1];
2155 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
2156 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
2157 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
2158 b_kstr, localB);
2159 obj->JointInternal.PositionLimitsInternal->data[0] = -3.10668606855;
2160 obj->JointInternal.PositionLimitsInternal->data
2161 [obj->JointInternal.PositionLimitsInternal->size[0]] = 3.10668606855;
2162 obj->JointInternal.JointAxisInternal[0] = 0.0;
2163 obj->JointInternal.JointAxisInternal[1] = 0.0;
2164 obj->JointInternal.JointAxisInternal[2] = 1.0;
2165 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
2166 obj->JointInternal.HomePositionInternal->size[0] = 1;
2167 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
2168 b_kstr, localB);
2169 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
2170 return b_obj;
2171}
2172
2173static v_robotics_manip_internal_Rig_T *carte_RigidBody_RigidBody_byapf
2174 (v_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB)
2175{
2176 v_robotics_manip_internal_Rig_T *b_obj;
2177 int8_T msubspace_data[36];
2178 real_T poslim_data[12];
2179 emxArray_char_T_cartesian_tra_T *switch_expression;
2180 boolean_T b_bool;
2181 int32_T b_kstr;
2182 char_T b[8];
2183 char_T b_0[9];
2184 int32_T loop_ub;
2185 int8_T tmp[6];
2186 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
2187 '5' };
2188
2189 static const real_T tmp_1[9] = { 0.012440329403329006, 2.388185677857016E-6,
2190 0.00012602126519218373, 2.388185677857016E-6, 0.012510746127660349,
2191 -9.077919321137075E-5, 0.00012602126519218373, -9.077919321137075E-5,
2192 0.00013851261456175015 };
2193
2194 static const real_T tmp_2[36] = { 0.012440329403329006, 2.388185677857016E-6,
2195 0.00012602126519218373, 0.0, -0.0021015312196166957, -3.5996159115316455E-5,
2196 2.388185677857016E-6, 0.012510746127660349, -9.077919321137075E-5,
2197 0.0021015312196166957, 0.0, -0.0023173509858408423, 0.00012602126519218373,
2198 -9.077919321137075E-5, 0.00013851261456175015, 3.5996159115316455E-5,
2199 0.0023173509858408423, 0.0, 0.0, 0.0021015312196166957,
2200 3.5996159115316455E-5, 0.0785942338762368, 0.0, 0.0, -0.0021015312196166957,
2201 0.0, 0.0023173509858408423, 0.0, 0.0785942338762368, 0.0,
2202 -3.5996159115316455E-5, -0.0023173509858408423, 0.0, 0.0, 0.0,
2203 0.0785942338762368 };
2204
2205 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
2206 1 };
2207
2208 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
2209 '_', '5' };
2210
2211 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
2212
2213 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
2214
2215 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
2216 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
2217
2218 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
2219 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
2220
2221 static const real_T tmp_9[36] = { 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
2222 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
2223 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
2224
2225 int32_T exitg1;
2226 b_obj = obj;
2227 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
2228 obj->NameInternal->size[0] = 1;
2229 obj->NameInternal->size[1] = 10;
2230 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr, localB);
2231 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
2232 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
2233 }
2234
2235 obj->ParentIndex = 5.0;
2236 obj->MassInternal = 0.0785942338762368;
2237 obj->CenterOfMassInternal[0] = 0.029485;
2238 obj->CenterOfMassInternal[1] = -0.000458;
2239 obj->CenterOfMassInternal[2] = 0.026739;
2240 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
2241 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
2242 }
2243
2244 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
2245 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
2246 }
2247
2248 obj->JointInternal.InTree = false;
2249 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
2250 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
2251 }
2252
2253 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
2254 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
2255 }
2256
2257 b_kstr = obj->JointInternal.NameInternal->size[0] *
2258 obj->JointInternal.NameInternal->size[1];
2259 obj->JointInternal.NameInternal->size[0] = 1;
2260 obj->JointInternal.NameInternal->size[1] = 11;
2261 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr,
2262 localB);
2263 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
2264 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
2265 }
2266
2267 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
2268 obj->JointInternal.Type->size[0] = 1;
2269 obj->JointInternal.Type->size[1] = 8;
2270 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr, localB);
2271 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
2272 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
2273 }
2274
2275 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
2276 b_kstr = switch_expression->size[0] * switch_expression->size[1];
2277 switch_expression->size[0] = 1;
2278 switch_expression->size[1] = obj->JointInternal.Type->size[1];
2279 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr, localB);
2280 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
2281 - 1;
2282 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
2283 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
2284 }
2285
2286 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
2287 b[b_kstr] = tmp_5[b_kstr];
2288 }
2289
2290 b_bool = false;
2291 if (switch_expression->size[1] == 8) {
2292 b_kstr = 1;
2293 do {
2294 exitg1 = 0;
2295 if (b_kstr - 1 < 8) {
2296 loop_ub = b_kstr - 1;
2297 if (switch_expression->data[loop_ub] != b[loop_ub]) {
2298 exitg1 = 1;
2299 } else {
2300 b_kstr++;
2301 }
2302 } else {
2303 b_bool = true;
2304 exitg1 = 1;
2305 }
2306 } while (exitg1 == 0);
2307 }
2308
2309 if (b_bool) {
2310 b_kstr = 0;
2311 } else {
2312 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
2313 b_0[b_kstr] = tmp_6[b_kstr];
2314 }
2315
2316 b_bool = false;
2317 if (switch_expression->size[1] == 9) {
2318 b_kstr = 1;
2319 do {
2320 exitg1 = 0;
2321 if (b_kstr - 1 < 9) {
2322 loop_ub = b_kstr - 1;
2323 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
2324 exitg1 = 1;
2325 } else {
2326 b_kstr++;
2327 }
2328 } else {
2329 b_bool = true;
2330 exitg1 = 1;
2331 }
2332 } while (exitg1 == 0);
2333 }
2334
2335 if (b_bool) {
2336 b_kstr = 1;
2337 } else {
2338 b_kstr = -1;
2339 }
2340 }
2341
2342 cartesian_trajec_emxFree_char_T(&switch_expression);
2343 switch (b_kstr) {
2344 case 0:
2345 tmp[0] = 0;
2346 tmp[1] = 0;
2347 tmp[2] = 1;
2348 tmp[3] = 0;
2349 tmp[4] = 0;
2350 tmp[5] = 0;
2351 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2352 msubspace_data[b_kstr] = tmp[b_kstr];
2353 }
2354
2355 poslim_data[0] = -3.1415926535897931;
2356 poslim_data[1] = 3.1415926535897931;
2357 obj->JointInternal.VelocityNumber = 1.0;
2358 obj->JointInternal.PositionNumber = 1.0;
2359 obj->JointInternal.JointAxisInternal[0] = 0.0;
2360 obj->JointInternal.JointAxisInternal[1] = 0.0;
2361 obj->JointInternal.JointAxisInternal[2] = 1.0;
2362 break;
2363
2364 case 1:
2365 tmp[0] = 0;
2366 tmp[1] = 0;
2367 tmp[2] = 0;
2368 tmp[3] = 0;
2369 tmp[4] = 0;
2370 tmp[5] = 1;
2371 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2372 msubspace_data[b_kstr] = tmp[b_kstr];
2373 }
2374
2375 poslim_data[0] = -0.5;
2376 poslim_data[1] = 0.5;
2377 obj->JointInternal.VelocityNumber = 1.0;
2378 obj->JointInternal.PositionNumber = 1.0;
2379 obj->JointInternal.JointAxisInternal[0] = 0.0;
2380 obj->JointInternal.JointAxisInternal[1] = 0.0;
2381 obj->JointInternal.JointAxisInternal[2] = 1.0;
2382 break;
2383
2384 default:
2385 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2386 msubspace_data[b_kstr] = 0;
2387 }
2388
2389 poslim_data[0] = 0.0;
2390 poslim_data[1] = 0.0;
2391 obj->JointInternal.VelocityNumber = 0.0;
2392 obj->JointInternal.PositionNumber = 0.0;
2393 obj->JointInternal.JointAxisInternal[0] = 0.0;
2394 obj->JointInternal.JointAxisInternal[1] = 0.0;
2395 obj->JointInternal.JointAxisInternal[2] = 0.0;
2396 break;
2397 }
2398
2399 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
2400 obj->JointInternal.MotionSubspace->size[1];
2401 obj->JointInternal.MotionSubspace->size[0] = 6;
2402 obj->JointInternal.MotionSubspace->size[1] = 1;
2403 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr,
2404 localB);
2405 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2406 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
2407 }
2408
2409 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
2410 obj->JointInternal.PositionLimitsInternal->size[1];
2411 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
2412 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
2413 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
2414 b_kstr, localB);
2415 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
2416 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
2417 }
2418
2419 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
2420 obj->JointInternal.HomePositionInternal->size[0] = 1;
2421 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
2422 b_kstr, localB);
2423 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
2424 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
2425 }
2426
2427 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
2428 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
2429 }
2430
2431 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
2432 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
2433 }
2434
2435 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
2436 obj->JointInternal.MotionSubspace->size[1];
2437 obj->JointInternal.MotionSubspace->size[0] = 6;
2438 obj->JointInternal.MotionSubspace->size[1] = 1;
2439 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr,
2440 localB);
2441 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2442 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
2443 }
2444
2445 obj->JointInternal.InTree = true;
2446 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
2447 obj->JointInternal.PositionLimitsInternal->size[1];
2448 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
2449 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
2450 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
2451 b_kstr, localB);
2452 obj->JointInternal.PositionLimitsInternal->data[0] = -1.79768912955;
2453 obj->JointInternal.PositionLimitsInternal->data
2454 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.79768912955;
2455 obj->JointInternal.JointAxisInternal[0] = 0.0;
2456 obj->JointInternal.JointAxisInternal[1] = 1.0;
2457 obj->JointInternal.JointAxisInternal[2] = 0.0;
2458 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
2459 obj->JointInternal.HomePositionInternal->size[0] = 1;
2460 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
2461 b_kstr, localB);
2462 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
2463 return b_obj;
2464}
2465
2466static v_robotics_manip_internal_Rig_T *cart_RigidBody_RigidBody_byapfa
2467 (v_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB)
2468{
2469 v_robotics_manip_internal_Rig_T *b_obj;
2470 int8_T msubspace_data[36];
2471 real_T poslim_data[12];
2472 emxArray_char_T_cartesian_tra_T *switch_expression;
2473 boolean_T b_bool;
2474 int32_T b_kstr;
2475 char_T b[8];
2476 char_T b_0[9];
2477 int32_T loop_ub;
2478 int8_T tmp[6];
2479 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
2480 '6' };
2481
2482 static const real_T tmp_1[9] = { 1.0067862401982823E-5, 2.0153545938371486E-9,
2483 5.3285284099072352E-9, 2.0153545938371486E-9, 1.4574493611914028E-5,
2484 1.6742291194075022E-9, 5.3285284099072352E-9, 1.6742291194075022E-9,
2485 1.006193323459457E-5 };
2486
2487 static const real_T tmp_2[36] = { 1.0067862401982823E-5, 2.0153545938371486E-9,
2488 5.3285284099072352E-9, 0.0, -1.6782149839359722E-7, -0.00026087851925284686,
2489 2.0153545938371486E-9, 1.4574493611914028E-5, 1.6742291194075022E-9,
2490 1.6782149839359722E-7, 0.0, -1.957917481258634E-7, 5.3285284099072352E-9,
2491 1.6742291194075022E-9, 1.006193323459457E-5, 0.00026087851925284686,
2492 1.957917481258634E-7, 0.0, 0.0, 1.6782149839359722E-7,
2493 0.00026087851925284686, 0.0279702497322662, 0.0, 0.0, -1.6782149839359722E-7,
2494 0.0, 1.957917481258634E-7, 0.0, 0.0279702497322662, 0.0,
2495 -0.00026087851925284686, -1.957917481258634E-7, 0.0, 0.0, 0.0,
2496 0.0279702497322662 };
2497
2498 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
2499 1 };
2500
2501 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
2502 '_', '6' };
2503
2504 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
2505
2506 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
2507
2508 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
2509 0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };
2510
2511 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
2512 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
2513
2514 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
2515 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
2516 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
2517
2518 int32_T exitg1;
2519 b_obj = obj;
2520 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
2521 obj->NameInternal->size[0] = 1;
2522 obj->NameInternal->size[1] = 10;
2523 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr, localB);
2524 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
2525 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
2526 }
2527
2528 obj->ParentIndex = 6.0;
2529 obj->MassInternal = 0.0279702497322662;
2530 obj->CenterOfMassInternal[0] = 7.0E-6;
2531 obj->CenterOfMassInternal[1] = -0.009327;
2532 obj->CenterOfMassInternal[2] = 6.0E-6;
2533 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
2534 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
2535 }
2536
2537 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
2538 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
2539 }
2540
2541 obj->JointInternal.InTree = false;
2542 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
2543 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
2544 }
2545
2546 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
2547 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
2548 }
2549
2550 b_kstr = obj->JointInternal.NameInternal->size[0] *
2551 obj->JointInternal.NameInternal->size[1];
2552 obj->JointInternal.NameInternal->size[0] = 1;
2553 obj->JointInternal.NameInternal->size[1] = 11;
2554 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr,
2555 localB);
2556 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
2557 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
2558 }
2559
2560 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
2561 obj->JointInternal.Type->size[0] = 1;
2562 obj->JointInternal.Type->size[1] = 8;
2563 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr, localB);
2564 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
2565 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
2566 }
2567
2568 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
2569 b_kstr = switch_expression->size[0] * switch_expression->size[1];
2570 switch_expression->size[0] = 1;
2571 switch_expression->size[1] = obj->JointInternal.Type->size[1];
2572 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr, localB);
2573 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
2574 - 1;
2575 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
2576 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
2577 }
2578
2579 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
2580 b[b_kstr] = tmp_5[b_kstr];
2581 }
2582
2583 b_bool = false;
2584 if (switch_expression->size[1] == 8) {
2585 b_kstr = 1;
2586 do {
2587 exitg1 = 0;
2588 if (b_kstr - 1 < 8) {
2589 loop_ub = b_kstr - 1;
2590 if (switch_expression->data[loop_ub] != b[loop_ub]) {
2591 exitg1 = 1;
2592 } else {
2593 b_kstr++;
2594 }
2595 } else {
2596 b_bool = true;
2597 exitg1 = 1;
2598 }
2599 } while (exitg1 == 0);
2600 }
2601
2602 if (b_bool) {
2603 b_kstr = 0;
2604 } else {
2605 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
2606 b_0[b_kstr] = tmp_6[b_kstr];
2607 }
2608
2609 b_bool = false;
2610 if (switch_expression->size[1] == 9) {
2611 b_kstr = 1;
2612 do {
2613 exitg1 = 0;
2614 if (b_kstr - 1 < 9) {
2615 loop_ub = b_kstr - 1;
2616 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
2617 exitg1 = 1;
2618 } else {
2619 b_kstr++;
2620 }
2621 } else {
2622 b_bool = true;
2623 exitg1 = 1;
2624 }
2625 } while (exitg1 == 0);
2626 }
2627
2628 if (b_bool) {
2629 b_kstr = 1;
2630 } else {
2631 b_kstr = -1;
2632 }
2633 }
2634
2635 cartesian_trajec_emxFree_char_T(&switch_expression);
2636 switch (b_kstr) {
2637 case 0:
2638 tmp[0] = 0;
2639 tmp[1] = 0;
2640 tmp[2] = 1;
2641 tmp[3] = 0;
2642 tmp[4] = 0;
2643 tmp[5] = 0;
2644 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2645 msubspace_data[b_kstr] = tmp[b_kstr];
2646 }
2647
2648 poslim_data[0] = -3.1415926535897931;
2649 poslim_data[1] = 3.1415926535897931;
2650 obj->JointInternal.VelocityNumber = 1.0;
2651 obj->JointInternal.PositionNumber = 1.0;
2652 obj->JointInternal.JointAxisInternal[0] = 0.0;
2653 obj->JointInternal.JointAxisInternal[1] = 0.0;
2654 obj->JointInternal.JointAxisInternal[2] = 1.0;
2655 break;
2656
2657 case 1:
2658 tmp[0] = 0;
2659 tmp[1] = 0;
2660 tmp[2] = 0;
2661 tmp[3] = 0;
2662 tmp[4] = 0;
2663 tmp[5] = 1;
2664 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2665 msubspace_data[b_kstr] = tmp[b_kstr];
2666 }
2667
2668 poslim_data[0] = -0.5;
2669 poslim_data[1] = 0.5;
2670 obj->JointInternal.VelocityNumber = 1.0;
2671 obj->JointInternal.PositionNumber = 1.0;
2672 obj->JointInternal.JointAxisInternal[0] = 0.0;
2673 obj->JointInternal.JointAxisInternal[1] = 0.0;
2674 obj->JointInternal.JointAxisInternal[2] = 1.0;
2675 break;
2676
2677 default:
2678 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2679 msubspace_data[b_kstr] = 0;
2680 }
2681
2682 poslim_data[0] = 0.0;
2683 poslim_data[1] = 0.0;
2684 obj->JointInternal.VelocityNumber = 0.0;
2685 obj->JointInternal.PositionNumber = 0.0;
2686 obj->JointInternal.JointAxisInternal[0] = 0.0;
2687 obj->JointInternal.JointAxisInternal[1] = 0.0;
2688 obj->JointInternal.JointAxisInternal[2] = 0.0;
2689 break;
2690 }
2691
2692 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
2693 obj->JointInternal.MotionSubspace->size[1];
2694 obj->JointInternal.MotionSubspace->size[0] = 6;
2695 obj->JointInternal.MotionSubspace->size[1] = 1;
2696 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr,
2697 localB);
2698 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2699 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
2700 }
2701
2702 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
2703 obj->JointInternal.PositionLimitsInternal->size[1];
2704 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
2705 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
2706 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
2707 b_kstr, localB);
2708 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
2709 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
2710 }
2711
2712 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
2713 obj->JointInternal.HomePositionInternal->size[0] = 1;
2714 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
2715 b_kstr, localB);
2716 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
2717 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
2718 }
2719
2720 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
2721 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
2722 }
2723
2724 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
2725 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
2726 }
2727
2728 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
2729 obj->JointInternal.MotionSubspace->size[1];
2730 obj->JointInternal.MotionSubspace->size[0] = 6;
2731 obj->JointInternal.MotionSubspace->size[1] = 1;
2732 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr,
2733 localB);
2734 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2735 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
2736 }
2737
2738 obj->JointInternal.InTree = true;
2739 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
2740 obj->JointInternal.PositionLimitsInternal->size[1];
2741 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
2742 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
2743 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
2744 b_kstr, localB);
2745 obj->JointInternal.PositionLimitsInternal->data[0] = -4.71238898038;
2746 obj->JointInternal.PositionLimitsInternal->data
2747 [obj->JointInternal.PositionLimitsInternal->size[0]] = 4.71238898038;
2748 obj->JointInternal.JointAxisInternal[0] = 0.0;
2749 obj->JointInternal.JointAxisInternal[1] = 0.0;
2750 obj->JointInternal.JointAxisInternal[2] = 1.0;
2751 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
2752 obj->JointInternal.HomePositionInternal->size[0] = 1;
2753 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
2754 b_kstr, localB);
2755 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
2756 return b_obj;
2757}
2758
2759static y_robotics_manip_internal_Rig_T *car_RigidBodyTree_RigidBodyTree
2760 (y_robotics_manip_internal_Rig_T *obj, v_robotics_manip_internal_Rig_T *iobj_0,
2761 v_robotics_manip_internal_Rig_T *iobj_1, v_robotics_manip_internal_Rig_T
2762 *iobj_2, v_robotics_manip_internal_Rig_T *iobj_3,
2763 v_robotics_manip_internal_Rig_T *iobj_4, v_robotics_manip_internal_Rig_T
2764 *iobj_5, v_robotics_manip_internal_Rig_T *iobj_6,
2765 v_robotics_manip_internal_Rig_T *iobj_7, B_MATLABSystem_cartesian_traj_T
2766 *localB)
2767{
2768 y_robotics_manip_internal_Rig_T *b_obj;
2769 v_robotics_manip_internal_Rig_T *obj_0;
2770 int8_T msubspace_data[36];
2771 real_T poslim_data[12];
2772 int8_T b_I[9];
2773 emxArray_char_T_cartesian_tra_T *switch_expression;
2774 boolean_T b_bool;
2775 int32_T b_kstr;
2776 char_T b[8];
2777 char_T b_0[9];
2778 int32_T loop_ub;
2779 int8_T tmp[6];
2780 static const char_T tmp_0[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
2781 'e', 'e' };
2782
2783 static const real_T tmp_1[36] = { 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0,
2784 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0,
2785 -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0 };
2786
2787 static const int8_T tmp_2[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
2788 1 };
2789
2790 static const char_T tmp_3[12] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
2791 '_', 'e', 'e' };
2792
2793 static const char_T tmp_4[5] = { 'f', 'i', 'x', 'e', 'd' };
2794
2795 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
2796
2797 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
2798
2799 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
2800 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
2801
2802 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
2803 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
2804
2805 static const char_T tmp_9[5] = { 'w', 'o', 'r', 'l', 'd' };
2806
2807 static const char_T tmp_a[9] = { 'w', 'o', 'r', 'l', 'd', '_', 'j', 'n', 't' };
2808
2809 int32_T exitg1;
2810 b_obj = obj;
2811 obj->Bodies[0] = cartesian_t_RigidBody_RigidBody(iobj_7, localB);
2812 obj->Bodies[0]->Index = 1.0;
2813 obj->Bodies[1] = cartesian_RigidBody_RigidBody_b(iobj_0, localB);
2814 obj->Bodies[1]->Index = 2.0;
2815 obj->Bodies[2] = cartesia_RigidBody_RigidBody_by(iobj_1, localB);
2816 obj->Bodies[2]->Index = 3.0;
2817 obj->Bodies[3] = cartesi_RigidBody_RigidBody_bya(iobj_2, localB);
2818 obj->Bodies[3]->Index = 4.0;
2819 obj->Bodies[4] = cartes_RigidBody_RigidBody_byap(iobj_3, localB);
2820 obj->Bodies[4]->Index = 5.0;
2821 obj->Bodies[5] = carte_RigidBody_RigidBody_byapf(iobj_4, localB);
2822 obj->Bodies[5]->Index = 6.0;
2823 obj->Bodies[6] = cart_RigidBody_RigidBody_byapfa(iobj_5, localB);
2824 obj->Bodies[6]->Index = 7.0;
2825 b_kstr = iobj_6->NameInternal->size[0] * iobj_6->NameInternal->size[1];
2826 iobj_6->NameInternal->size[0] = 1;
2827 iobj_6->NameInternal->size[1] = 11;
2828 cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal, b_kstr, localB);
2829 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
2830 iobj_6->NameInternal->data[b_kstr] = tmp_0[b_kstr];
2831 }
2832
2833 iobj_6->ParentIndex = 7.0;
2834 iobj_6->MassInternal = 0.0;
2835 iobj_6->CenterOfMassInternal[0] = 0.0;
2836 iobj_6->CenterOfMassInternal[1] = 0.0;
2837 iobj_6->CenterOfMassInternal[2] = 0.0;
2838 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
2839 iobj_6->InertiaInternal[b_kstr] = 0.0;
2840 }
2841
2842 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
2843 iobj_6->SpatialInertia[b_kstr] = tmp_1[b_kstr];
2844 }
2845
2846 iobj_6->JointInternal.InTree = false;
2847 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
2848 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
2849 }
2850
2851 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
2852 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_2[b_kstr];
2853 }
2854
2855 b_kstr = iobj_6->JointInternal.NameInternal->size[0] *
2856 iobj_6->JointInternal.NameInternal->size[1];
2857 iobj_6->JointInternal.NameInternal->size[0] = 1;
2858 iobj_6->JointInternal.NameInternal->size[1] = 12;
2859 cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.NameInternal, b_kstr,
2860 localB);
2861 for (b_kstr = 0; b_kstr < 12; b_kstr++) {
2862 iobj_6->JointInternal.NameInternal->data[b_kstr] = tmp_3[b_kstr];
2863 }
2864
2865 b_kstr = iobj_6->JointInternal.Type->size[0] * iobj_6->
2866 JointInternal.Type->size[1];
2867 iobj_6->JointInternal.Type->size[0] = 1;
2868 iobj_6->JointInternal.Type->size[1] = 5;
2869 cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.Type, b_kstr, localB);
2870 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
2871 iobj_6->JointInternal.Type->data[b_kstr] = tmp_4[b_kstr];
2872 }
2873
2874 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
2875 b_kstr = switch_expression->size[0] * switch_expression->size[1];
2876 switch_expression->size[0] = 1;
2877 switch_expression->size[1] = iobj_6->JointInternal.Type->size[1];
2878 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr, localB);
2879 loop_ub = iobj_6->JointInternal.Type->size[0] * iobj_6->
2880 JointInternal.Type->size[1] - 1;
2881 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
2882 switch_expression->data[b_kstr] = iobj_6->JointInternal.Type->data[b_kstr];
2883 }
2884
2885 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
2886 b[b_kstr] = tmp_5[b_kstr];
2887 }
2888
2889 b_bool = false;
2890 if (switch_expression->size[1] == 8) {
2891 b_kstr = 1;
2892 do {
2893 exitg1 = 0;
2894 if (b_kstr - 1 < 8) {
2895 loop_ub = b_kstr - 1;
2896 if (switch_expression->data[loop_ub] != b[loop_ub]) {
2897 exitg1 = 1;
2898 } else {
2899 b_kstr++;
2900 }
2901 } else {
2902 b_bool = true;
2903 exitg1 = 1;
2904 }
2905 } while (exitg1 == 0);
2906 }
2907
2908 if (b_bool) {
2909 b_kstr = 0;
2910 } else {
2911 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
2912 b_0[b_kstr] = tmp_6[b_kstr];
2913 }
2914
2915 b_bool = false;
2916 if (switch_expression->size[1] == 9) {
2917 b_kstr = 1;
2918 do {
2919 exitg1 = 0;
2920 if (b_kstr - 1 < 9) {
2921 loop_ub = b_kstr - 1;
2922 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
2923 exitg1 = 1;
2924 } else {
2925 b_kstr++;
2926 }
2927 } else {
2928 b_bool = true;
2929 exitg1 = 1;
2930 }
2931 } while (exitg1 == 0);
2932 }
2933
2934 if (b_bool) {
2935 b_kstr = 1;
2936 } else {
2937 b_kstr = -1;
2938 }
2939 }
2940
2941 switch (b_kstr) {
2942 case 0:
2943 tmp[0] = 0;
2944 tmp[1] = 0;
2945 tmp[2] = 1;
2946 tmp[3] = 0;
2947 tmp[4] = 0;
2948 tmp[5] = 0;
2949 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2950 msubspace_data[b_kstr] = tmp[b_kstr];
2951 }
2952
2953 poslim_data[0] = -3.1415926535897931;
2954 poslim_data[1] = 3.1415926535897931;
2955 iobj_6->JointInternal.VelocityNumber = 1.0;
2956 iobj_6->JointInternal.PositionNumber = 1.0;
2957 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
2958 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
2959 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
2960 break;
2961
2962 case 1:
2963 tmp[0] = 0;
2964 tmp[1] = 0;
2965 tmp[2] = 0;
2966 tmp[3] = 0;
2967 tmp[4] = 0;
2968 tmp[5] = 1;
2969 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2970 msubspace_data[b_kstr] = tmp[b_kstr];
2971 }
2972
2973 poslim_data[0] = -0.5;
2974 poslim_data[1] = 0.5;
2975 iobj_6->JointInternal.VelocityNumber = 1.0;
2976 iobj_6->JointInternal.PositionNumber = 1.0;
2977 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
2978 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
2979 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
2980 break;
2981
2982 default:
2983 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
2984 msubspace_data[b_kstr] = 0;
2985 }
2986
2987 poslim_data[0] = 0.0;
2988 poslim_data[1] = 0.0;
2989 iobj_6->JointInternal.VelocityNumber = 0.0;
2990 iobj_6->JointInternal.PositionNumber = 0.0;
2991 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
2992 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
2993 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
2994 break;
2995 }
2996
2997 b_kstr = iobj_6->JointInternal.MotionSubspace->size[0] *
2998 iobj_6->JointInternal.MotionSubspace->size[1];
2999 iobj_6->JointInternal.MotionSubspace->size[0] = 6;
3000 iobj_6->JointInternal.MotionSubspace->size[1] = 1;
3001 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.MotionSubspace, b_kstr,
3002 localB);
3003 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3004 iobj_6->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
3005 }
3006
3007 b_kstr = iobj_6->JointInternal.PositionLimitsInternal->size[0] *
3008 iobj_6->JointInternal.PositionLimitsInternal->size[1];
3009 iobj_6->JointInternal.PositionLimitsInternal->size[0] = 1;
3010 iobj_6->JointInternal.PositionLimitsInternal->size[1] = 2;
3011 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.PositionLimitsInternal,
3012 b_kstr, localB);
3013 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
3014 iobj_6->JointInternal.PositionLimitsInternal->data[b_kstr] =
3015 poslim_data[b_kstr];
3016 }
3017
3018 b_kstr = iobj_6->JointInternal.HomePositionInternal->size[0];
3019 iobj_6->JointInternal.HomePositionInternal->size[0] = 1;
3020 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.HomePositionInternal,
3021 b_kstr, localB);
3022 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
3023 iobj_6->JointInternal.HomePositionInternal->data[0] = 0.0;
3024 }
3025
3026 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
3027 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
3028 }
3029
3030 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
3031 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
3032 }
3033
3034 b_kstr = iobj_6->JointInternal.MotionSubspace->size[0] *
3035 iobj_6->JointInternal.MotionSubspace->size[1];
3036 iobj_6->JointInternal.MotionSubspace->size[0] = 6;
3037 iobj_6->JointInternal.MotionSubspace->size[1] = 1;
3038 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.MotionSubspace, b_kstr,
3039 localB);
3040 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3041 iobj_6->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
3042 }
3043
3044 iobj_6->JointInternal.InTree = true;
3045 b_kstr = iobj_6->JointInternal.PositionLimitsInternal->size[0] *
3046 iobj_6->JointInternal.PositionLimitsInternal->size[1];
3047 iobj_6->JointInternal.PositionLimitsInternal->size[0] = 1;
3048 iobj_6->JointInternal.PositionLimitsInternal->size[1] = 2;
3049 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.PositionLimitsInternal,
3050 b_kstr, localB);
3051 iobj_6->JointInternal.PositionLimitsInternal->data[0] = 0.0;
3052 iobj_6->JointInternal.PositionLimitsInternal->data
3053 [iobj_6->JointInternal.PositionLimitsInternal->size[0]] = 0.0;
3054 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
3055 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
3056 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
3057 b_kstr = iobj_6->JointInternal.HomePositionInternal->size[0];
3058 iobj_6->JointInternal.HomePositionInternal->size[0] = 1;
3059 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.HomePositionInternal,
3060 b_kstr, localB);
3061 iobj_6->JointInternal.HomePositionInternal->data[0] = 0.0;
3062 obj->Bodies[7] = iobj_6;
3063 obj->Bodies[7]->Index = 8.0;
3064 obj->NumBodies = 8.0;
3065 obj->Gravity[0] = 0.0;
3066 obj->Gravity[1] = 0.0;
3067 obj->Gravity[2] = 0.0;
3068 obj_0 = &obj->Base;
3069 b_kstr = obj->Base.NameInternal->size[0] * obj->Base.NameInternal->size[1];
3070 obj->Base.NameInternal->size[0] = 1;
3071 obj->Base.NameInternal->size[1] = 5;
3072 cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal, b_kstr, localB);
3073 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
3074 obj->Base.NameInternal->data[b_kstr] = tmp_9[b_kstr];
3075 }
3076
3077 obj->Base.JointInternal.InTree = false;
3078 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
3079 obj_0->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
3080 }
3081
3082 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
3083 obj_0->JointInternal.ChildToJointTransform[b_kstr] = tmp_2[b_kstr];
3084 }
3085
3086 b_kstr = obj->Base.JointInternal.NameInternal->size[0] *
3087 obj->Base.JointInternal.NameInternal->size[1];
3088 obj->Base.JointInternal.NameInternal->size[0] = 1;
3089 obj->Base.JointInternal.NameInternal->size[1] = 9;
3090 cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.NameInternal, b_kstr,
3091 localB);
3092 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
3093 obj_0->JointInternal.NameInternal->data[b_kstr] = tmp_a[b_kstr];
3094 }
3095
3096 b_kstr = obj->Base.JointInternal.Type->size[0] * obj->
3097 Base.JointInternal.Type->size[1];
3098 obj->Base.JointInternal.Type->size[0] = 1;
3099 obj->Base.JointInternal.Type->size[1] = 5;
3100 cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.Type, b_kstr, localB);
3101 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
3102 obj_0->JointInternal.Type->data[b_kstr] = tmp_4[b_kstr];
3103 }
3104
3105 b_kstr = switch_expression->size[0] * switch_expression->size[1];
3106 switch_expression->size[0] = 1;
3107 switch_expression->size[1] = obj->Base.JointInternal.Type->size[1];
3108 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr, localB);
3109 loop_ub = obj->Base.JointInternal.Type->size[0] * obj->
3110 Base.JointInternal.Type->size[1] - 1;
3111 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
3112 switch_expression->data[b_kstr] = obj_0->JointInternal.Type->data[b_kstr];
3113 }
3114
3115 b_bool = false;
3116 if (switch_expression->size[1] == 8) {
3117 b_kstr = 1;
3118 do {
3119 exitg1 = 0;
3120 if (b_kstr - 1 < 8) {
3121 loop_ub = b_kstr - 1;
3122 if (switch_expression->data[loop_ub] != b[loop_ub]) {
3123 exitg1 = 1;
3124 } else {
3125 b_kstr++;
3126 }
3127 } else {
3128 b_bool = true;
3129 exitg1 = 1;
3130 }
3131 } while (exitg1 == 0);
3132 }
3133
3134 if (b_bool) {
3135 b_kstr = 0;
3136 } else {
3137 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
3138 b_0[b_kstr] = tmp_6[b_kstr];
3139 }
3140
3141 b_bool = false;
3142 if (switch_expression->size[1] == 9) {
3143 b_kstr = 1;
3144 do {
3145 exitg1 = 0;
3146 if (b_kstr - 1 < 9) {
3147 loop_ub = b_kstr - 1;
3148 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
3149 exitg1 = 1;
3150 } else {
3151 b_kstr++;
3152 }
3153 } else {
3154 b_bool = true;
3155 exitg1 = 1;
3156 }
3157 } while (exitg1 == 0);
3158 }
3159
3160 if (b_bool) {
3161 b_kstr = 1;
3162 } else {
3163 b_kstr = -1;
3164 }
3165 }
3166
3167 cartesian_trajec_emxFree_char_T(&switch_expression);
3168 switch (b_kstr) {
3169 case 0:
3170 tmp[0] = 0;
3171 tmp[1] = 0;
3172 tmp[2] = 1;
3173 tmp[3] = 0;
3174 tmp[4] = 0;
3175 tmp[5] = 0;
3176 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3177 msubspace_data[b_kstr] = tmp[b_kstr];
3178 }
3179
3180 poslim_data[0] = -3.1415926535897931;
3181 poslim_data[1] = 3.1415926535897931;
3182 obj->Base.JointInternal.VelocityNumber = 1.0;
3183 obj->Base.JointInternal.PositionNumber = 1.0;
3184 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
3185 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
3186 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
3187 break;
3188
3189 case 1:
3190 tmp[0] = 0;
3191 tmp[1] = 0;
3192 tmp[2] = 0;
3193 tmp[3] = 0;
3194 tmp[4] = 0;
3195 tmp[5] = 1;
3196 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3197 msubspace_data[b_kstr] = tmp[b_kstr];
3198 }
3199
3200 poslim_data[0] = -0.5;
3201 poslim_data[1] = 0.5;
3202 obj->Base.JointInternal.VelocityNumber = 1.0;
3203 obj->Base.JointInternal.PositionNumber = 1.0;
3204 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
3205 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
3206 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
3207 break;
3208
3209 default:
3210 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3211 msubspace_data[b_kstr] = 0;
3212 }
3213
3214 poslim_data[0] = 0.0;
3215 poslim_data[1] = 0.0;
3216 obj->Base.JointInternal.VelocityNumber = 0.0;
3217 obj->Base.JointInternal.PositionNumber = 0.0;
3218 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
3219 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
3220 obj->Base.JointInternal.JointAxisInternal[2] = 0.0;
3221 break;
3222 }
3223
3224 b_kstr = obj->Base.JointInternal.MotionSubspace->size[0] *
3225 obj->Base.JointInternal.MotionSubspace->size[1];
3226 obj->Base.JointInternal.MotionSubspace->size[0] = 6;
3227 obj->Base.JointInternal.MotionSubspace->size[1] = 1;
3228 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.MotionSubspace, b_kstr,
3229 localB);
3230 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3231 obj_0->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
3232 }
3233
3234 b_kstr = obj->Base.JointInternal.PositionLimitsInternal->size[0] *
3235 obj->Base.JointInternal.PositionLimitsInternal->size[1];
3236 obj->Base.JointInternal.PositionLimitsInternal->size[0] = 1;
3237 obj->Base.JointInternal.PositionLimitsInternal->size[1] = 2;
3238 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.PositionLimitsInternal,
3239 b_kstr, localB);
3240 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
3241 obj_0->JointInternal.PositionLimitsInternal->data[b_kstr] =
3242 poslim_data[b_kstr];
3243 }
3244
3245 b_kstr = obj->Base.JointInternal.HomePositionInternal->size[0];
3246 obj->Base.JointInternal.HomePositionInternal->size[0] = 1;
3247 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.HomePositionInternal,
3248 b_kstr, localB);
3249 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
3250 obj_0->JointInternal.HomePositionInternal->data[0] = 0.0;
3251 }
3252
3253 obj->Base.Index = -1.0;
3254 obj->Base.ParentIndex = -1.0;
3255 obj->Base.MassInternal = 1.0;
3256 obj->Base.CenterOfMassInternal[0] = 0.0;
3257 obj->Base.CenterOfMassInternal[1] = 0.0;
3258 obj->Base.CenterOfMassInternal[2] = 0.0;
3259 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
3260 b_I[b_kstr] = 0;
3261 }
3262
3263 b_I[0] = 1;
3264 b_I[4] = 1;
3265 b_I[8] = 1;
3266 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
3267 obj->Base.InertiaInternal[b_kstr] = b_I[b_kstr];
3268 }
3269
3270 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
3271 msubspace_data[b_kstr] = 0;
3272 }
3273
3274 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3275 msubspace_data[b_kstr + 6 * b_kstr] = 1;
3276 }
3277
3278 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
3279 obj->Base.SpatialInertia[b_kstr] = msubspace_data[b_kstr];
3280 }
3281
3282 return b_obj;
3283}
3284
3285static void cartesi_genrand_uint32_vector_d(uint32_T mt[625], uint32_T u[2],
3286 B_MATLABSystem_cartesian_traj_T *localB)
3287{
3288 for (localB->b_j_i = 0; localB->b_j_i < 2; localB->b_j_i++) {
3289 localB->mti = mt[624] + 1U;
3290 if (localB->mti >= 625U) {
3291 for (localB->b_kk = 0; localB->b_kk < 227; localB->b_kk++) {
3292 localB->y_d = (mt[localB->b_kk + 1] & 2147483647U) | (mt[localB->b_kk] &
3293 2147483648U);
3294 if ((localB->y_d & 1U) == 0U) {
3295 localB->y_d >>= 1U;
3296 } else {
3297 localB->y_d = localB->y_d >> 1U ^ 2567483615U;
3298 }
3299
3300 mt[localB->b_kk] = mt[localB->b_kk + 397] ^ localB->y_d;
3301 }
3302
3303 for (localB->b_kk = 0; localB->b_kk < 396; localB->b_kk++) {
3304 localB->y_d = (mt[localB->b_kk + 227] & 2147483648U) | (mt[localB->b_kk
3305 + 228] & 2147483647U);
3306 if ((localB->y_d & 1U) == 0U) {
3307 localB->y_d >>= 1U;
3308 } else {
3309 localB->y_d = localB->y_d >> 1U ^ 2567483615U;
3310 }
3311
3312 mt[localB->b_kk + 227] = mt[localB->b_kk] ^ localB->y_d;
3313 }
3314
3315 localB->y_d = (mt[623] & 2147483648U) | (mt[0] & 2147483647U);
3316 if ((localB->y_d & 1U) == 0U) {
3317 localB->y_d >>= 1U;
3318 } else {
3319 localB->y_d = localB->y_d >> 1U ^ 2567483615U;
3320 }
3321
3322 mt[623] = mt[396] ^ localB->y_d;
3323 localB->mti = 1U;
3324 }
3325
3326 localB->y_d = mt[static_cast<int32_T>(localB->mti) - 1];
3327 mt[624] = localB->mti;
3328 localB->y_d ^= localB->y_d >> 11U;
3329 localB->y_d ^= localB->y_d << 7U & 2636928640U;
3330 localB->y_d ^= localB->y_d << 15U & 4022730752U;
3331 u[localB->b_j_i] = localB->y_d >> 18U ^ localB->y_d;
3332 }
3333}
3334
3335static boolean_T cartesian_trajec_is_valid_state(const uint32_T mt[625],
3336 B_MATLABSystem_cartesian_traj_T *localB)
3337{
3338 boolean_T isvalid;
3339 boolean_T exitg1;
3340 if ((mt[624] >= 1U) && (mt[624] < 625U)) {
3341 isvalid = true;
3342 } else {
3343 isvalid = false;
3344 }
3345
3346 if (isvalid) {
3347 isvalid = false;
3348 localB->k_f = 0;
3349 exitg1 = false;
3350 while ((!exitg1) && (localB->k_f + 1 < 625)) {
3351 if (mt[localB->k_f] == 0U) {
3352 localB->k_f++;
3353 } else {
3354 isvalid = true;
3355 exitg1 = true;
3356 }
3357 }
3358 }
3359
3360 return isvalid;
3361}
3362
3363static void cartesian__eml_rand_mt19937ar_a(const uint32_T state[625], uint32_T
3364 b_state[625], real_T *r, B_MATLABSystem_cartesian_traj_T *localB)
3365{
3366 int32_T exitg1;
3367 memcpy(&b_state[0], &state[0], 625U * sizeof(uint32_T));
3368
3369 // ========================= COPYRIGHT NOTICE ============================
3370 // This is a uniform (0,1) pseudorandom number generator based on:
3371 //
3372 // A C-program for MT19937, with initialization improved 2002/1/26.
3373 // Coded by Takuji Nishimura and Makoto Matsumoto.
3374 //
3375 // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
3376 // All rights reserved.
3377 //
3378 // Redistribution and use in source and binary forms, with or without
3379 // modification, are permitted provided that the following conditions
3380 // are met:
3381 //
3382 // 1. Redistributions of source code must retain the above copyright
3383 // notice, this list of conditions and the following disclaimer.
3384 //
3385 // 2. Redistributions in binary form must reproduce the above copyright
3386 // notice, this list of conditions and the following disclaimer
3387 // in the documentation and/or other materials provided with the
3388 // distribution.
3389 //
3390 // 3. The names of its contributors may not be used to endorse or
3391 // promote products derived from this software without specific
3392 // prior written permission.
3393 //
3394 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
3395 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
3396 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
3397 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
3398 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
3399 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
3400 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
3401 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
3402 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
3403 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
3404 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
3405 //
3406 // ============================= END =================================
3407 do {
3408 exitg1 = 0;
3409 cartesi_genrand_uint32_vector_d(b_state, localB->b_u, localB);
3410 *r = (static_cast<real_T>(localB->b_u[0] >> 5U) * 6.7108864E+7 +
3411 static_cast<real_T>(localB->b_u[1] >> 6U)) * 1.1102230246251565E-16;
3412 if (*r == 0.0) {
3413 if (!cartesian_trajec_is_valid_state(b_state, localB)) {
3414 localB->r = 5489U;
3415 b_state[0] = 5489U;
3416 for (localB->b_mti = 0; localB->b_mti < 623; localB->b_mti++) {
3417 localB->r = ((localB->r >> 30U ^ localB->r) * 1812433253U +
3418 localB->b_mti) + 1U;
3419 b_state[localB->b_mti + 1] = localB->r;
3420 }
3421
3422 b_state[624] = 624U;
3423 }
3424 } else {
3425 exitg1 = 1;
3426 }
3427 } while (exitg1 == 0);
3428}
3429
3430static void cartesian_trajectory_plann_rand(real_T r[5],
3431 B_MATLABSystem_cartesian_traj_T *localB, DW_MATLABSystem_cartesian_tra_T
3432 *localDW)
3433{
3434 for (localB->b_k_h = 0; localB->b_k_h < 5; localB->b_k_h++) {
3435 memcpy(&localB->uv[0], &localDW->state_e[0], 625U * sizeof(uint32_T));
3436 cartesian__eml_rand_mt19937ar_a(localB->uv, localDW->state_e, &r
3437 [localB->b_k_h], localB);
3438 }
3439}
3440
3441static w_robotics_manip_internal_Rig_T *car_RigidBody_RigidBody_byapfac
3442 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_tr_T *iobj_0,
3443 B_MATLABSystem_cartesian_traj_T *localB)
3444{
3445 w_robotics_manip_internal_Rig_T *b_obj;
3446 int8_T msubspace_data[36];
3447 real_T poslim_data[12];
3448 emxArray_char_T_cartesian_tra_T *switch_expression;
3449 boolean_T b_bool;
3450 int32_T b_kstr;
3451 char_T b[8];
3452 char_T b_0[9];
3453 int32_T loop_ub;
3454 int8_T tmp[6];
3455 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
3456 '\x01' };
3457
3458 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
3459 1 };
3460
3461 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
3462 '\x01', '_', 'j', 'n', 't' };
3463
3464 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
3465
3466 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
3467
3468 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
3469
3470 int32_T exitg1;
3471 b_obj = obj;
3472 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
3473 obj->NameInternal->size[0] = 1;
3474 obj->NameInternal->size[1] = 10;
3475 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr, localB);
3476 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
3477 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
3478 }
3479
3480 iobj_0->InTree = false;
3481 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
3482 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
3483 }
3484
3485 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
3486 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
3487 }
3488
3489 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
3490 iobj_0->NameInternal->size[0] = 1;
3491 iobj_0->NameInternal->size[1] = 14;
3492 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr, localB);
3493 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
3494 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
3495 }
3496
3497 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
3498 iobj_0->Type->size[0] = 1;
3499 iobj_0->Type->size[1] = 5;
3500 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr, localB);
3501 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
3502 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
3503 }
3504
3505 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
3506 b_kstr = switch_expression->size[0] * switch_expression->size[1];
3507 switch_expression->size[0] = 1;
3508 switch_expression->size[1] = iobj_0->Type->size[1];
3509 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr, localB);
3510 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
3511 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
3512 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
3513 }
3514
3515 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
3516 b[b_kstr] = tmp_4[b_kstr];
3517 }
3518
3519 b_bool = false;
3520 if (switch_expression->size[1] == 8) {
3521 b_kstr = 1;
3522 do {
3523 exitg1 = 0;
3524 if (b_kstr - 1 < 8) {
3525 loop_ub = b_kstr - 1;
3526 if (switch_expression->data[loop_ub] != b[loop_ub]) {
3527 exitg1 = 1;
3528 } else {
3529 b_kstr++;
3530 }
3531 } else {
3532 b_bool = true;
3533 exitg1 = 1;
3534 }
3535 } while (exitg1 == 0);
3536 }
3537
3538 if (b_bool) {
3539 b_kstr = 0;
3540 } else {
3541 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
3542 b_0[b_kstr] = tmp_5[b_kstr];
3543 }
3544
3545 b_bool = false;
3546 if (switch_expression->size[1] == 9) {
3547 b_kstr = 1;
3548 do {
3549 exitg1 = 0;
3550 if (b_kstr - 1 < 9) {
3551 loop_ub = b_kstr - 1;
3552 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
3553 exitg1 = 1;
3554 } else {
3555 b_kstr++;
3556 }
3557 } else {
3558 b_bool = true;
3559 exitg1 = 1;
3560 }
3561 } while (exitg1 == 0);
3562 }
3563
3564 if (b_bool) {
3565 b_kstr = 1;
3566 } else {
3567 b_kstr = -1;
3568 }
3569 }
3570
3571 cartesian_trajec_emxFree_char_T(&switch_expression);
3572 switch (b_kstr) {
3573 case 0:
3574 tmp[0] = 0;
3575 tmp[1] = 0;
3576 tmp[2] = 1;
3577 tmp[3] = 0;
3578 tmp[4] = 0;
3579 tmp[5] = 0;
3580 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3581 msubspace_data[b_kstr] = tmp[b_kstr];
3582 }
3583
3584 poslim_data[0] = -3.1415926535897931;
3585 poslim_data[1] = 3.1415926535897931;
3586 iobj_0->VelocityNumber = 1.0;
3587 iobj_0->PositionNumber = 1.0;
3588 iobj_0->JointAxisInternal[0] = 0.0;
3589 iobj_0->JointAxisInternal[1] = 0.0;
3590 iobj_0->JointAxisInternal[2] = 1.0;
3591 break;
3592
3593 case 1:
3594 tmp[0] = 0;
3595 tmp[1] = 0;
3596 tmp[2] = 0;
3597 tmp[3] = 0;
3598 tmp[4] = 0;
3599 tmp[5] = 1;
3600 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3601 msubspace_data[b_kstr] = tmp[b_kstr];
3602 }
3603
3604 poslim_data[0] = -0.5;
3605 poslim_data[1] = 0.5;
3606 iobj_0->VelocityNumber = 1.0;
3607 iobj_0->PositionNumber = 1.0;
3608 iobj_0->JointAxisInternal[0] = 0.0;
3609 iobj_0->JointAxisInternal[1] = 0.0;
3610 iobj_0->JointAxisInternal[2] = 1.0;
3611 break;
3612
3613 default:
3614 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3615 msubspace_data[b_kstr] = 0;
3616 }
3617
3618 poslim_data[0] = 0.0;
3619 poslim_data[1] = 0.0;
3620 iobj_0->VelocityNumber = 0.0;
3621 iobj_0->PositionNumber = 0.0;
3622 iobj_0->JointAxisInternal[0] = 0.0;
3623 iobj_0->JointAxisInternal[1] = 0.0;
3624 iobj_0->JointAxisInternal[2] = 0.0;
3625 break;
3626 }
3627
3628 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
3629 iobj_0->MotionSubspace->size[0] = 6;
3630 iobj_0->MotionSubspace->size[1] = 1;
3631 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr, localB);
3632 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3633 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
3634 }
3635
3636 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
3637 iobj_0->PositionLimitsInternal->size[1];
3638 iobj_0->PositionLimitsInternal->size[0] = 1;
3639 iobj_0->PositionLimitsInternal->size[1] = 2;
3640 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr, localB);
3641 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
3642 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
3643 }
3644
3645 b_kstr = iobj_0->HomePositionInternal->size[0];
3646 iobj_0->HomePositionInternal->size[0] = 1;
3647 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr, localB);
3648 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
3649 iobj_0->HomePositionInternal->data[0] = 0.0;
3650 }
3651
3652 obj->JointInternal = iobj_0;
3653 obj->Index = -1.0;
3654 obj->ParentIndex = -1.0;
3655 return b_obj;
3656}
3657
3658static w_robotics_manip_internal_Rig_T *ca_RigidBody_RigidBody_byapfac3
3659 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_tr_T *iobj_0,
3660 B_MATLABSystem_cartesian_traj_T *localB)
3661{
3662 w_robotics_manip_internal_Rig_T *b_obj;
3663 int8_T msubspace_data[36];
3664 real_T poslim_data[12];
3665 emxArray_char_T_cartesian_tra_T *switch_expression;
3666 boolean_T b_bool;
3667 int32_T b_kstr;
3668 char_T b[8];
3669 char_T b_0[9];
3670 int32_T loop_ub;
3671 int8_T tmp[6];
3672 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
3673 '\x02' };
3674
3675 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
3676 1 };
3677
3678 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
3679 '\x02', '_', 'j', 'n', 't' };
3680
3681 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
3682
3683 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
3684
3685 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
3686
3687 int32_T exitg1;
3688 b_obj = obj;
3689 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
3690 obj->NameInternal->size[0] = 1;
3691 obj->NameInternal->size[1] = 10;
3692 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr, localB);
3693 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
3694 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
3695 }
3696
3697 iobj_0->InTree = false;
3698 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
3699 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
3700 }
3701
3702 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
3703 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
3704 }
3705
3706 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
3707 iobj_0->NameInternal->size[0] = 1;
3708 iobj_0->NameInternal->size[1] = 14;
3709 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr, localB);
3710 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
3711 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
3712 }
3713
3714 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
3715 iobj_0->Type->size[0] = 1;
3716 iobj_0->Type->size[1] = 5;
3717 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr, localB);
3718 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
3719 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
3720 }
3721
3722 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
3723 b_kstr = switch_expression->size[0] * switch_expression->size[1];
3724 switch_expression->size[0] = 1;
3725 switch_expression->size[1] = iobj_0->Type->size[1];
3726 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr, localB);
3727 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
3728 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
3729 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
3730 }
3731
3732 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
3733 b[b_kstr] = tmp_4[b_kstr];
3734 }
3735
3736 b_bool = false;
3737 if (switch_expression->size[1] == 8) {
3738 b_kstr = 1;
3739 do {
3740 exitg1 = 0;
3741 if (b_kstr - 1 < 8) {
3742 loop_ub = b_kstr - 1;
3743 if (switch_expression->data[loop_ub] != b[loop_ub]) {
3744 exitg1 = 1;
3745 } else {
3746 b_kstr++;
3747 }
3748 } else {
3749 b_bool = true;
3750 exitg1 = 1;
3751 }
3752 } while (exitg1 == 0);
3753 }
3754
3755 if (b_bool) {
3756 b_kstr = 0;
3757 } else {
3758 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
3759 b_0[b_kstr] = tmp_5[b_kstr];
3760 }
3761
3762 b_bool = false;
3763 if (switch_expression->size[1] == 9) {
3764 b_kstr = 1;
3765 do {
3766 exitg1 = 0;
3767 if (b_kstr - 1 < 9) {
3768 loop_ub = b_kstr - 1;
3769 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
3770 exitg1 = 1;
3771 } else {
3772 b_kstr++;
3773 }
3774 } else {
3775 b_bool = true;
3776 exitg1 = 1;
3777 }
3778 } while (exitg1 == 0);
3779 }
3780
3781 if (b_bool) {
3782 b_kstr = 1;
3783 } else {
3784 b_kstr = -1;
3785 }
3786 }
3787
3788 cartesian_trajec_emxFree_char_T(&switch_expression);
3789 switch (b_kstr) {
3790 case 0:
3791 tmp[0] = 0;
3792 tmp[1] = 0;
3793 tmp[2] = 1;
3794 tmp[3] = 0;
3795 tmp[4] = 0;
3796 tmp[5] = 0;
3797 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3798 msubspace_data[b_kstr] = tmp[b_kstr];
3799 }
3800
3801 poslim_data[0] = -3.1415926535897931;
3802 poslim_data[1] = 3.1415926535897931;
3803 iobj_0->VelocityNumber = 1.0;
3804 iobj_0->PositionNumber = 1.0;
3805 iobj_0->JointAxisInternal[0] = 0.0;
3806 iobj_0->JointAxisInternal[1] = 0.0;
3807 iobj_0->JointAxisInternal[2] = 1.0;
3808 break;
3809
3810 case 1:
3811 tmp[0] = 0;
3812 tmp[1] = 0;
3813 tmp[2] = 0;
3814 tmp[3] = 0;
3815 tmp[4] = 0;
3816 tmp[5] = 1;
3817 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3818 msubspace_data[b_kstr] = tmp[b_kstr];
3819 }
3820
3821 poslim_data[0] = -0.5;
3822 poslim_data[1] = 0.5;
3823 iobj_0->VelocityNumber = 1.0;
3824 iobj_0->PositionNumber = 1.0;
3825 iobj_0->JointAxisInternal[0] = 0.0;
3826 iobj_0->JointAxisInternal[1] = 0.0;
3827 iobj_0->JointAxisInternal[2] = 1.0;
3828 break;
3829
3830 default:
3831 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3832 msubspace_data[b_kstr] = 0;
3833 }
3834
3835 poslim_data[0] = 0.0;
3836 poslim_data[1] = 0.0;
3837 iobj_0->VelocityNumber = 0.0;
3838 iobj_0->PositionNumber = 0.0;
3839 iobj_0->JointAxisInternal[0] = 0.0;
3840 iobj_0->JointAxisInternal[1] = 0.0;
3841 iobj_0->JointAxisInternal[2] = 0.0;
3842 break;
3843 }
3844
3845 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
3846 iobj_0->MotionSubspace->size[0] = 6;
3847 iobj_0->MotionSubspace->size[1] = 1;
3848 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr, localB);
3849 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3850 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
3851 }
3852
3853 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
3854 iobj_0->PositionLimitsInternal->size[1];
3855 iobj_0->PositionLimitsInternal->size[0] = 1;
3856 iobj_0->PositionLimitsInternal->size[1] = 2;
3857 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr, localB);
3858 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
3859 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
3860 }
3861
3862 b_kstr = iobj_0->HomePositionInternal->size[0];
3863 iobj_0->HomePositionInternal->size[0] = 1;
3864 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr, localB);
3865 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
3866 iobj_0->HomePositionInternal->data[0] = 0.0;
3867 }
3868
3869 obj->JointInternal = iobj_0;
3870 obj->Index = -1.0;
3871 obj->ParentIndex = -1.0;
3872 return b_obj;
3873}
3874
3875static w_robotics_manip_internal_Rig_T *c_RigidBody_RigidBody_byapfac3i
3876 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_tr_T *iobj_0,
3877 B_MATLABSystem_cartesian_traj_T *localB)
3878{
3879 w_robotics_manip_internal_Rig_T *b_obj;
3880 int8_T msubspace_data[36];
3881 real_T poslim_data[12];
3882 emxArray_char_T_cartesian_tra_T *switch_expression;
3883 boolean_T b_bool;
3884 int32_T b_kstr;
3885 char_T b[8];
3886 char_T b_0[9];
3887 int32_T loop_ub;
3888 int8_T tmp[6];
3889 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
3890 '\x03' };
3891
3892 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
3893 1 };
3894
3895 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
3896 '\x03', '_', 'j', 'n', 't' };
3897
3898 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
3899
3900 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
3901
3902 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
3903
3904 int32_T exitg1;
3905 b_obj = obj;
3906 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
3907 obj->NameInternal->size[0] = 1;
3908 obj->NameInternal->size[1] = 10;
3909 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr, localB);
3910 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
3911 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
3912 }
3913
3914 iobj_0->InTree = false;
3915 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
3916 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
3917 }
3918
3919 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
3920 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
3921 }
3922
3923 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
3924 iobj_0->NameInternal->size[0] = 1;
3925 iobj_0->NameInternal->size[1] = 14;
3926 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr, localB);
3927 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
3928 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
3929 }
3930
3931 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
3932 iobj_0->Type->size[0] = 1;
3933 iobj_0->Type->size[1] = 5;
3934 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr, localB);
3935 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
3936 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
3937 }
3938
3939 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
3940 b_kstr = switch_expression->size[0] * switch_expression->size[1];
3941 switch_expression->size[0] = 1;
3942 switch_expression->size[1] = iobj_0->Type->size[1];
3943 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr, localB);
3944 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
3945 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
3946 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
3947 }
3948
3949 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
3950 b[b_kstr] = tmp_4[b_kstr];
3951 }
3952
3953 b_bool = false;
3954 if (switch_expression->size[1] == 8) {
3955 b_kstr = 1;
3956 do {
3957 exitg1 = 0;
3958 if (b_kstr - 1 < 8) {
3959 loop_ub = b_kstr - 1;
3960 if (switch_expression->data[loop_ub] != b[loop_ub]) {
3961 exitg1 = 1;
3962 } else {
3963 b_kstr++;
3964 }
3965 } else {
3966 b_bool = true;
3967 exitg1 = 1;
3968 }
3969 } while (exitg1 == 0);
3970 }
3971
3972 if (b_bool) {
3973 b_kstr = 0;
3974 } else {
3975 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
3976 b_0[b_kstr] = tmp_5[b_kstr];
3977 }
3978
3979 b_bool = false;
3980 if (switch_expression->size[1] == 9) {
3981 b_kstr = 1;
3982 do {
3983 exitg1 = 0;
3984 if (b_kstr - 1 < 9) {
3985 loop_ub = b_kstr - 1;
3986 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
3987 exitg1 = 1;
3988 } else {
3989 b_kstr++;
3990 }
3991 } else {
3992 b_bool = true;
3993 exitg1 = 1;
3994 }
3995 } while (exitg1 == 0);
3996 }
3997
3998 if (b_bool) {
3999 b_kstr = 1;
4000 } else {
4001 b_kstr = -1;
4002 }
4003 }
4004
4005 cartesian_trajec_emxFree_char_T(&switch_expression);
4006 switch (b_kstr) {
4007 case 0:
4008 tmp[0] = 0;
4009 tmp[1] = 0;
4010 tmp[2] = 1;
4011 tmp[3] = 0;
4012 tmp[4] = 0;
4013 tmp[5] = 0;
4014 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
4015 msubspace_data[b_kstr] = tmp[b_kstr];
4016 }
4017
4018 poslim_data[0] = -3.1415926535897931;
4019 poslim_data[1] = 3.1415926535897931;
4020 iobj_0->VelocityNumber = 1.0;
4021 iobj_0->PositionNumber = 1.0;
4022 iobj_0->JointAxisInternal[0] = 0.0;
4023 iobj_0->JointAxisInternal[1] = 0.0;
4024 iobj_0->JointAxisInternal[2] = 1.0;
4025 break;
4026
4027 case 1:
4028 tmp[0] = 0;
4029 tmp[1] = 0;
4030 tmp[2] = 0;
4031 tmp[3] = 0;
4032 tmp[4] = 0;
4033 tmp[5] = 1;
4034 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
4035 msubspace_data[b_kstr] = tmp[b_kstr];
4036 }
4037
4038 poslim_data[0] = -0.5;
4039 poslim_data[1] = 0.5;
4040 iobj_0->VelocityNumber = 1.0;
4041 iobj_0->PositionNumber = 1.0;
4042 iobj_0->JointAxisInternal[0] = 0.0;
4043 iobj_0->JointAxisInternal[1] = 0.0;
4044 iobj_0->JointAxisInternal[2] = 1.0;
4045 break;
4046
4047 default:
4048 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
4049 msubspace_data[b_kstr] = 0;
4050 }
4051
4052 poslim_data[0] = 0.0;
4053 poslim_data[1] = 0.0;
4054 iobj_0->VelocityNumber = 0.0;
4055 iobj_0->PositionNumber = 0.0;
4056 iobj_0->JointAxisInternal[0] = 0.0;
4057 iobj_0->JointAxisInternal[1] = 0.0;
4058 iobj_0->JointAxisInternal[2] = 0.0;
4059 break;
4060 }
4061
4062 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
4063 iobj_0->MotionSubspace->size[0] = 6;
4064 iobj_0->MotionSubspace->size[1] = 1;
4065 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr, localB);
4066 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
4067 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
4068 }
4069
4070 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
4071 iobj_0->PositionLimitsInternal->size[1];
4072 iobj_0->PositionLimitsInternal->size[0] = 1;
4073 iobj_0->PositionLimitsInternal->size[1] = 2;
4074 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr, localB);
4075 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
4076 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
4077 }
4078
4079 b_kstr = iobj_0->HomePositionInternal->size[0];
4080 iobj_0->HomePositionInternal->size[0] = 1;
4081 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr, localB);
4082 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
4083 iobj_0->HomePositionInternal->data[0] = 0.0;
4084 }
4085
4086 obj->JointInternal = iobj_0;
4087 obj->Index = -1.0;
4088 obj->ParentIndex = -1.0;
4089 return b_obj;
4090}
4091
4092static w_robotics_manip_internal_Rig_T *RigidBody_RigidBody_byapfac3iu
4093 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_tr_T *iobj_0,
4094 B_MATLABSystem_cartesian_traj_T *localB)
4095{
4096 w_robotics_manip_internal_Rig_T *b_obj;
4097 int8_T msubspace_data[36];
4098 real_T poslim_data[12];
4099 emxArray_char_T_cartesian_tra_T *switch_expression;
4100 boolean_T b_bool;
4101 int32_T b_kstr;
4102 char_T b[8];
4103 char_T b_0[9];
4104 int32_T loop_ub;
4105 int8_T tmp[6];
4106 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
4107 '\x04' };
4108
4109 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
4110 1 };
4111
4112 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
4113 '\x04', '_', 'j', 'n', 't' };
4114
4115 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
4116
4117 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
4118
4119 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
4120
4121 int32_T exitg1;
4122 b_obj = obj;
4123 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
4124 obj->NameInternal->size[0] = 1;
4125 obj->NameInternal->size[1] = 10;
4126 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr, localB);
4127 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
4128 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
4129 }
4130
4131 iobj_0->InTree = false;
4132 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
4133 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
4134 }
4135
4136 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
4137 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
4138 }
4139
4140 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
4141 iobj_0->NameInternal->size[0] = 1;
4142 iobj_0->NameInternal->size[1] = 14;
4143 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr, localB);
4144 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
4145 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
4146 }
4147
4148 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
4149 iobj_0->Type->size[0] = 1;
4150 iobj_0->Type->size[1] = 5;
4151 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr, localB);
4152 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
4153 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
4154 }
4155
4156 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
4157 b_kstr = switch_expression->size[0] * switch_expression->size[1];
4158 switch_expression->size[0] = 1;
4159 switch_expression->size[1] = iobj_0->Type->size[1];
4160 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr, localB);
4161 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
4162 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
4163 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
4164 }
4165
4166 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
4167 b[b_kstr] = tmp_4[b_kstr];
4168 }
4169
4170 b_bool = false;
4171 if (switch_expression->size[1] == 8) {
4172 b_kstr = 1;
4173 do {
4174 exitg1 = 0;
4175 if (b_kstr - 1 < 8) {
4176 loop_ub = b_kstr - 1;
4177 if (switch_expression->data[loop_ub] != b[loop_ub]) {
4178 exitg1 = 1;
4179 } else {
4180 b_kstr++;
4181 }
4182 } else {
4183 b_bool = true;
4184 exitg1 = 1;
4185 }
4186 } while (exitg1 == 0);
4187 }
4188
4189 if (b_bool) {
4190 b_kstr = 0;
4191 } else {
4192 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
4193 b_0[b_kstr] = tmp_5[b_kstr];
4194 }
4195
4196 b_bool = false;
4197 if (switch_expression->size[1] == 9) {
4198 b_kstr = 1;
4199 do {
4200 exitg1 = 0;
4201 if (b_kstr - 1 < 9) {
4202 loop_ub = b_kstr - 1;
4203 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
4204 exitg1 = 1;
4205 } else {
4206 b_kstr++;
4207 }
4208 } else {
4209 b_bool = true;
4210 exitg1 = 1;
4211 }
4212 } while (exitg1 == 0);
4213 }
4214
4215 if (b_bool) {
4216 b_kstr = 1;
4217 } else {
4218 b_kstr = -1;
4219 }
4220 }
4221
4222 cartesian_trajec_emxFree_char_T(&switch_expression);
4223 switch (b_kstr) {
4224 case 0:
4225 tmp[0] = 0;
4226 tmp[1] = 0;
4227 tmp[2] = 1;
4228 tmp[3] = 0;
4229 tmp[4] = 0;
4230 tmp[5] = 0;
4231 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
4232 msubspace_data[b_kstr] = tmp[b_kstr];
4233 }
4234
4235 poslim_data[0] = -3.1415926535897931;
4236 poslim_data[1] = 3.1415926535897931;
4237 iobj_0->VelocityNumber = 1.0;
4238 iobj_0->PositionNumber = 1.0;
4239 iobj_0->JointAxisInternal[0] = 0.0;
4240 iobj_0->JointAxisInternal[1] = 0.0;
4241 iobj_0->JointAxisInternal[2] = 1.0;
4242 break;
4243
4244 case 1:
4245 tmp[0] = 0;
4246 tmp[1] = 0;
4247 tmp[2] = 0;
4248 tmp[3] = 0;
4249 tmp[4] = 0;
4250 tmp[5] = 1;
4251 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
4252 msubspace_data[b_kstr] = tmp[b_kstr];
4253 }
4254
4255 poslim_data[0] = -0.5;
4256 poslim_data[1] = 0.5;
4257 iobj_0->VelocityNumber = 1.0;
4258 iobj_0->PositionNumber = 1.0;
4259 iobj_0->JointAxisInternal[0] = 0.0;
4260 iobj_0->JointAxisInternal[1] = 0.0;
4261 iobj_0->JointAxisInternal[2] = 1.0;
4262 break;
4263
4264 default:
4265 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
4266 msubspace_data[b_kstr] = 0;
4267 }
4268
4269 poslim_data[0] = 0.0;
4270 poslim_data[1] = 0.0;
4271 iobj_0->VelocityNumber = 0.0;
4272 iobj_0->PositionNumber = 0.0;
4273 iobj_0->JointAxisInternal[0] = 0.0;
4274 iobj_0->JointAxisInternal[1] = 0.0;
4275 iobj_0->JointAxisInternal[2] = 0.0;
4276 break;
4277 }
4278
4279 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
4280 iobj_0->MotionSubspace->size[0] = 6;
4281 iobj_0->MotionSubspace->size[1] = 1;
4282 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr, localB);
4283 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
4284 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
4285 }
4286
4287 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
4288 iobj_0->PositionLimitsInternal->size[1];
4289 iobj_0->PositionLimitsInternal->size[0] = 1;
4290 iobj_0->PositionLimitsInternal->size[1] = 2;
4291 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr, localB);
4292 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
4293 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
4294 }
4295
4296 b_kstr = iobj_0->HomePositionInternal->size[0];
4297 iobj_0->HomePositionInternal->size[0] = 1;
4298 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr, localB);
4299 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
4300 iobj_0->HomePositionInternal->data[0] = 0.0;
4301 }
4302
4303 obj->JointInternal = iobj_0;
4304 obj->Index = -1.0;
4305 obj->ParentIndex = -1.0;
4306 return b_obj;
4307}
4308
4309static w_robotics_manip_internal_Rig_T *RigidBody_RigidBody_byapfac3iu2
4310 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_tr_T *iobj_0,
4311 B_MATLABSystem_cartesian_traj_T *localB)
4312{
4313 w_robotics_manip_internal_Rig_T *b_obj;
4314 int8_T msubspace_data[36];
4315 real_T poslim_data[12];
4316 emxArray_char_T_cartesian_tra_T *switch_expression;
4317 boolean_T b_bool;
4318 int32_T b_kstr;
4319 char_T b[8];
4320 char_T b_0[9];
4321 int32_T loop_ub;
4322 int8_T tmp[6];
4323 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
4324 '\x05' };
4325
4326 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
4327 1 };
4328
4329 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
4330 '\x05', '_', 'j', 'n', 't' };
4331
4332 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
4333
4334 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
4335
4336 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
4337
4338 int32_T exitg1;
4339 b_obj = obj;
4340 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
4341 obj->NameInternal->size[0] = 1;
4342 obj->NameInternal->size[1] = 10;
4343 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr, localB);
4344 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
4345 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
4346 }
4347
4348 iobj_0->InTree = false;
4349 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
4350 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
4351 }
4352
4353 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
4354 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
4355 }
4356
4357 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
4358 iobj_0->NameInternal->size[0] = 1;
4359 iobj_0->NameInternal->size[1] = 14;
4360 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr, localB);
4361 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
4362 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
4363 }
4364
4365 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
4366 iobj_0->Type->size[0] = 1;
4367 iobj_0->Type->size[1] = 5;
4368 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr, localB);
4369 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
4370 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
4371 }
4372
4373 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
4374 b_kstr = switch_expression->size[0] * switch_expression->size[1];
4375 switch_expression->size[0] = 1;
4376 switch_expression->size[1] = iobj_0->Type->size[1];
4377 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr, localB);
4378 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
4379 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
4380 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
4381 }
4382
4383 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
4384 b[b_kstr] = tmp_4[b_kstr];
4385 }
4386
4387 b_bool = false;
4388 if (switch_expression->size[1] == 8) {
4389 b_kstr = 1;
4390 do {
4391 exitg1 = 0;
4392 if (b_kstr - 1 < 8) {
4393 loop_ub = b_kstr - 1;
4394 if (switch_expression->data[loop_ub] != b[loop_ub]) {
4395 exitg1 = 1;
4396 } else {
4397 b_kstr++;
4398 }
4399 } else {
4400 b_bool = true;
4401 exitg1 = 1;
4402 }
4403 } while (exitg1 == 0);
4404 }
4405
4406 if (b_bool) {
4407 b_kstr = 0;
4408 } else {
4409 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
4410 b_0[b_kstr] = tmp_5[b_kstr];
4411 }
4412
4413 b_bool = false;
4414 if (switch_expression->size[1] == 9) {
4415 b_kstr = 1;
4416 do {
4417 exitg1 = 0;
4418 if (b_kstr - 1 < 9) {
4419 loop_ub = b_kstr - 1;
4420 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
4421 exitg1 = 1;
4422 } else {
4423 b_kstr++;
4424 }
4425 } else {
4426 b_bool = true;
4427 exitg1 = 1;
4428 }
4429 } while (exitg1 == 0);
4430 }
4431
4432 if (b_bool) {
4433 b_kstr = 1;
4434 } else {
4435 b_kstr = -1;
4436 }
4437 }
4438
4439 cartesian_trajec_emxFree_char_T(&switch_expression);
4440 switch (b_kstr) {
4441 case 0:
4442 tmp[0] = 0;
4443 tmp[1] = 0;
4444 tmp[2] = 1;
4445 tmp[3] = 0;
4446 tmp[4] = 0;
4447 tmp[5] = 0;
4448 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
4449 msubspace_data[b_kstr] = tmp[b_kstr];
4450 }
4451
4452 poslim_data[0] = -3.1415926535897931;
4453 poslim_data[1] = 3.1415926535897931;
4454 iobj_0->VelocityNumber = 1.0;
4455 iobj_0->PositionNumber = 1.0;
4456 iobj_0->JointAxisInternal[0] = 0.0;
4457 iobj_0->JointAxisInternal[1] = 0.0;
4458 iobj_0->JointAxisInternal[2] = 1.0;
4459 break;
4460
4461 case 1:
4462 tmp[0] = 0;
4463 tmp[1] = 0;
4464 tmp[2] = 0;
4465 tmp[3] = 0;
4466 tmp[4] = 0;
4467 tmp[5] = 1;
4468 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
4469 msubspace_data[b_kstr] = tmp[b_kstr];
4470 }
4471
4472 poslim_data[0] = -0.5;
4473 poslim_data[1] = 0.5;
4474 iobj_0->VelocityNumber = 1.0;
4475 iobj_0->PositionNumber = 1.0;
4476 iobj_0->JointAxisInternal[0] = 0.0;
4477 iobj_0->JointAxisInternal[1] = 0.0;
4478 iobj_0->JointAxisInternal[2] = 1.0;
4479 break;
4480
4481 default:
4482 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
4483 msubspace_data[b_kstr] = 0;
4484 }
4485
4486 poslim_data[0] = 0.0;
4487 poslim_data[1] = 0.0;
4488 iobj_0->VelocityNumber = 0.0;
4489 iobj_0->PositionNumber = 0.0;
4490 iobj_0->JointAxisInternal[0] = 0.0;
4491 iobj_0->JointAxisInternal[1] = 0.0;
4492 iobj_0->JointAxisInternal[2] = 0.0;
4493 break;
4494 }
4495
4496 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
4497 iobj_0->MotionSubspace->size[0] = 6;
4498 iobj_0->MotionSubspace->size[1] = 1;
4499 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr, localB);
4500 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
4501 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
4502 }
4503
4504 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
4505 iobj_0->PositionLimitsInternal->size[1];
4506 iobj_0->PositionLimitsInternal->size[0] = 1;
4507 iobj_0->PositionLimitsInternal->size[1] = 2;
4508 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr, localB);
4509 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
4510 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
4511 }
4512
4513 b_kstr = iobj_0->HomePositionInternal->size[0];
4514 iobj_0->HomePositionInternal->size[0] = 1;
4515 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr, localB);
4516 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
4517 iobj_0->HomePositionInternal->data[0] = 0.0;
4518 }
4519
4520 obj->JointInternal = iobj_0;
4521 obj->Index = -1.0;
4522 obj->ParentIndex = -1.0;
4523 return b_obj;
4524}
4525
4526static void ca_RigidBodyTree_clearAllBodies(x_robotics_manip_internal_Rig_T *obj,
4527 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
4528 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
4529 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
4530 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
4531 w_robotics_manip_internal_Rig_T *iobj_6, c_rigidBodyJoint_cartesian_tr_T
4532 *iobj_7, c_rigidBodyJoint_cartesian_tr_T *iobj_8,
4533 c_rigidBodyJoint_cartesian_tr_T *iobj_9, c_rigidBodyJoint_cartesian_tr_T
4534 *iobj_10, c_rigidBodyJoint_cartesian_tr_T *iobj_11,
4535 c_rigidBodyJoint_cartesian_tr_T *iobj_12, c_rigidBodyJoint_cartesian_tr_T
4536 *iobj_13, c_rigidBodyJoint_cartesian_tr_T *iobj_14,
4537 w_robotics_manip_internal_Rig_T *iobj_15, B_MATLABSystem_cartesian_traj_T
4538 *localB, DW_MATLABSystem_cartesian_tra_T *localDW)
4539{
4540 emxArray_char_T_cartesian_tra_T *switch_expression;
4541 static const char_T tmp[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
4542 '\x06' };
4543
4544 static const int8_T tmp_0[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
4545 1 };
4546
4547 static const char_T tmp_1[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
4548 '\x06', '_', 'j', 'n', 't' };
4549
4550 static const char_T tmp_2[5] = { 'f', 'i', 'x', 'e', 'd' };
4551
4552 static const char_T tmp_3[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
4553
4554 static const char_T tmp_4[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
4555
4556 static const char_T tmp_5[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
4557 '\x07' };
4558
4559 static const char_T tmp_6[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
4560 '\x07', '_', 'j', 'n', 't' };
4561
4562 static const char_T tmp_7[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
4563 '\x08' };
4564
4565 static const char_T tmp_8[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
4566 '\x08', '_', 'j', 'n', 't' };
4567
4568 int32_T exitg1;
4569 obj->Bodies[0] = car_RigidBody_RigidBody_byapfac(iobj_15, iobj_14, localB);
4570 obj->Bodies[1] = ca_RigidBody_RigidBody_byapfac3(iobj_0, iobj_7, localB);
4571 obj->Bodies[2] = c_RigidBody_RigidBody_byapfac3i(iobj_1, iobj_8, localB);
4572 obj->Bodies[3] = RigidBody_RigidBody_byapfac3iu(iobj_2, iobj_9, localB);
4573 obj->Bodies[4] = RigidBody_RigidBody_byapfac3iu2(iobj_3, iobj_10, localB);
4574 localB->b_kstr_o = iobj_4->NameInternal->size[0] * iobj_4->NameInternal->size
4575 [1];
4576 iobj_4->NameInternal->size[0] = 1;
4577 iobj_4->NameInternal->size[1] = 10;
4578 cartes_emxEnsureCapacity_char_T(iobj_4->NameInternal, localB->b_kstr_o, localB);
4579 for (localB->b_kstr_o = 0; localB->b_kstr_o < 10; localB->b_kstr_o++) {
4580 iobj_4->NameInternal->data[localB->b_kstr_o] = tmp[localB->b_kstr_o];
4581 }
4582
4583 iobj_11->InTree = false;
4584 for (localB->b_kstr_o = 0; localB->b_kstr_o < 16; localB->b_kstr_o++) {
4585 iobj_11->JointToParentTransform[localB->b_kstr_o] = tmp_0[localB->b_kstr_o];
4586 }
4587
4588 for (localB->b_kstr_o = 0; localB->b_kstr_o < 16; localB->b_kstr_o++) {
4589 iobj_11->ChildToJointTransform[localB->b_kstr_o] = tmp_0[localB->b_kstr_o];
4590 }
4591
4592 localB->b_kstr_o = iobj_11->NameInternal->size[0] * iobj_11->
4593 NameInternal->size[1];
4594 iobj_11->NameInternal->size[0] = 1;
4595 iobj_11->NameInternal->size[1] = 14;
4596 cartes_emxEnsureCapacity_char_T(iobj_11->NameInternal, localB->b_kstr_o,
4597 localB);
4598 for (localB->b_kstr_o = 0; localB->b_kstr_o < 14; localB->b_kstr_o++) {
4599 iobj_11->NameInternal->data[localB->b_kstr_o] = tmp_1[localB->b_kstr_o];
4600 }
4601
4602 localB->b_kstr_o = iobj_11->Type->size[0] * iobj_11->Type->size[1];
4603 iobj_11->Type->size[0] = 1;
4604 iobj_11->Type->size[1] = 5;
4605 cartes_emxEnsureCapacity_char_T(iobj_11->Type, localB->b_kstr_o, localB);
4606 for (localB->b_kstr_o = 0; localB->b_kstr_o < 5; localB->b_kstr_o++) {
4607 iobj_11->Type->data[localB->b_kstr_o] = tmp_2[localB->b_kstr_o];
4608 }
4609
4610 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
4611 localB->b_kstr_o = switch_expression->size[0] * switch_expression->size[1];
4612 switch_expression->size[0] = 1;
4613 switch_expression->size[1] = iobj_11->Type->size[1];
4614 cartes_emxEnsureCapacity_char_T(switch_expression, localB->b_kstr_o, localB);
4615 localB->loop_ub_c = iobj_11->Type->size[0] * iobj_11->Type->size[1] - 1;
4616 for (localB->b_kstr_o = 0; localB->b_kstr_o <= localB->loop_ub_c;
4617 localB->b_kstr_o++) {
4618 switch_expression->data[localB->b_kstr_o] = iobj_11->Type->data
4619 [localB->b_kstr_o];
4620 }
4621
4622 for (localB->b_kstr_o = 0; localB->b_kstr_o < 8; localB->b_kstr_o++) {
4623 localB->b_p[localB->b_kstr_o] = tmp_3[localB->b_kstr_o];
4624 }
4625
4626 localB->b_bool_kb = false;
4627 if (switch_expression->size[1] == 8) {
4628 localB->b_kstr_o = 1;
4629 do {
4630 exitg1 = 0;
4631 if (localB->b_kstr_o - 1 < 8) {
4632 localB->loop_ub_c = localB->b_kstr_o - 1;
4633 if (switch_expression->data[localB->loop_ub_c] != localB->b_p
4634 [localB->loop_ub_c]) {
4635 exitg1 = 1;
4636 } else {
4637 localB->b_kstr_o++;
4638 }
4639 } else {
4640 localB->b_bool_kb = true;
4641 exitg1 = 1;
4642 }
4643 } while (exitg1 == 0);
4644 }
4645
4646 if (localB->b_bool_kb) {
4647 localB->b_kstr_o = 0;
4648 } else {
4649 for (localB->b_kstr_o = 0; localB->b_kstr_o < 9; localB->b_kstr_o++) {
4650 localB->b_m[localB->b_kstr_o] = tmp_4[localB->b_kstr_o];
4651 }
4652
4653 localB->b_bool_kb = false;
4654 if (switch_expression->size[1] == 9) {
4655 localB->b_kstr_o = 1;
4656 do {
4657 exitg1 = 0;
4658 if (localB->b_kstr_o - 1 < 9) {
4659 localB->loop_ub_c = localB->b_kstr_o - 1;
4660 if (switch_expression->data[localB->loop_ub_c] != localB->b_m
4661 [localB->loop_ub_c]) {
4662 exitg1 = 1;
4663 } else {
4664 localB->b_kstr_o++;
4665 }
4666 } else {
4667 localB->b_bool_kb = true;
4668 exitg1 = 1;
4669 }
4670 } while (exitg1 == 0);
4671 }
4672
4673 if (localB->b_bool_kb) {
4674 localB->b_kstr_o = 1;
4675 } else {
4676 localB->b_kstr_o = -1;
4677 }
4678 }
4679
4680 switch (localB->b_kstr_o) {
4681 case 0:
4682 localB->iv1[0] = 0;
4683 localB->iv1[1] = 0;
4684 localB->iv1[2] = 1;
4685 localB->iv1[3] = 0;
4686 localB->iv1[4] = 0;
4687 localB->iv1[5] = 0;
4688 for (localB->b_kstr_o = 0; localB->b_kstr_o < 6; localB->b_kstr_o++) {
4689 localB->msubspace_data_a[localB->b_kstr_o] = localB->iv1[localB->b_kstr_o];
4690 }
4691
4692 localB->poslim_data_c[0] = -3.1415926535897931;
4693 localB->poslim_data_c[1] = 3.1415926535897931;
4694 iobj_11->VelocityNumber = 1.0;
4695 iobj_11->PositionNumber = 1.0;
4696 iobj_11->JointAxisInternal[0] = 0.0;
4697 iobj_11->JointAxisInternal[1] = 0.0;
4698 iobj_11->JointAxisInternal[2] = 1.0;
4699 break;
4700
4701 case 1:
4702 localB->iv1[0] = 0;
4703 localB->iv1[1] = 0;
4704 localB->iv1[2] = 0;
4705 localB->iv1[3] = 0;
4706 localB->iv1[4] = 0;
4707 localB->iv1[5] = 1;
4708 for (localB->b_kstr_o = 0; localB->b_kstr_o < 6; localB->b_kstr_o++) {
4709 localB->msubspace_data_a[localB->b_kstr_o] = localB->iv1[localB->b_kstr_o];
4710 }
4711
4712 localB->poslim_data_c[0] = -0.5;
4713 localB->poslim_data_c[1] = 0.5;
4714 iobj_11->VelocityNumber = 1.0;
4715 iobj_11->PositionNumber = 1.0;
4716 iobj_11->JointAxisInternal[0] = 0.0;
4717 iobj_11->JointAxisInternal[1] = 0.0;
4718 iobj_11->JointAxisInternal[2] = 1.0;
4719 break;
4720
4721 default:
4722 for (localB->b_kstr_o = 0; localB->b_kstr_o < 6; localB->b_kstr_o++) {
4723 localB->msubspace_data_a[localB->b_kstr_o] = 0;
4724 }
4725
4726 localB->poslim_data_c[0] = 0.0;
4727 localB->poslim_data_c[1] = 0.0;
4728 iobj_11->VelocityNumber = 0.0;
4729 iobj_11->PositionNumber = 0.0;
4730 iobj_11->JointAxisInternal[0] = 0.0;
4731 iobj_11->JointAxisInternal[1] = 0.0;
4732 iobj_11->JointAxisInternal[2] = 0.0;
4733 break;
4734 }
4735
4736 localB->b_kstr_o = iobj_11->MotionSubspace->size[0] * iobj_11->
4737 MotionSubspace->size[1];
4738 iobj_11->MotionSubspace->size[0] = 6;
4739 iobj_11->MotionSubspace->size[1] = 1;
4740 cartes_emxEnsureCapacity_real_T(iobj_11->MotionSubspace, localB->b_kstr_o,
4741 localB);
4742 for (localB->b_kstr_o = 0; localB->b_kstr_o < 6; localB->b_kstr_o++) {
4743 iobj_11->MotionSubspace->data[localB->b_kstr_o] = localB->
4744 msubspace_data_a[localB->b_kstr_o];
4745 }
4746
4747 localB->b_kstr_o = iobj_11->PositionLimitsInternal->size[0] *
4748 iobj_11->PositionLimitsInternal->size[1];
4749 iobj_11->PositionLimitsInternal->size[0] = 1;
4750 iobj_11->PositionLimitsInternal->size[1] = 2;
4751 cartes_emxEnsureCapacity_real_T(iobj_11->PositionLimitsInternal,
4752 localB->b_kstr_o, localB);
4753 for (localB->b_kstr_o = 0; localB->b_kstr_o < 2; localB->b_kstr_o++) {
4754 iobj_11->PositionLimitsInternal->data[localB->b_kstr_o] =
4755 localB->poslim_data_c[localB->b_kstr_o];
4756 }
4757
4758 localB->b_kstr_o = iobj_11->HomePositionInternal->size[0];
4759 iobj_11->HomePositionInternal->size[0] = 1;
4760 cartes_emxEnsureCapacity_real_T(iobj_11->HomePositionInternal,
4761 localB->b_kstr_o, localB);
4762 for (localB->b_kstr_o = 0; localB->b_kstr_o < 1; localB->b_kstr_o++) {
4763 iobj_11->HomePositionInternal->data[0] = 0.0;
4764 }
4765
4766 iobj_4->JointInternal = iobj_11;
4767 iobj_4->Index = -1.0;
4768 iobj_4->ParentIndex = -1.0;
4769 obj->Bodies[5] = iobj_4;
4770 localB->b_kstr_o = iobj_5->NameInternal->size[0] * iobj_5->NameInternal->size
4771 [1];
4772 iobj_5->NameInternal->size[0] = 1;
4773 iobj_5->NameInternal->size[1] = 10;
4774 cartes_emxEnsureCapacity_char_T(iobj_5->NameInternal, localB->b_kstr_o, localB);
4775 for (localB->b_kstr_o = 0; localB->b_kstr_o < 10; localB->b_kstr_o++) {
4776 iobj_5->NameInternal->data[localB->b_kstr_o] = tmp_5[localB->b_kstr_o];
4777 }
4778
4779 iobj_12->InTree = false;
4780 for (localB->b_kstr_o = 0; localB->b_kstr_o < 16; localB->b_kstr_o++) {
4781 iobj_12->JointToParentTransform[localB->b_kstr_o] = tmp_0[localB->b_kstr_o];
4782 }
4783
4784 for (localB->b_kstr_o = 0; localB->b_kstr_o < 16; localB->b_kstr_o++) {
4785 iobj_12->ChildToJointTransform[localB->b_kstr_o] = tmp_0[localB->b_kstr_o];
4786 }
4787
4788 localB->b_kstr_o = iobj_12->NameInternal->size[0] * iobj_12->
4789 NameInternal->size[1];
4790 iobj_12->NameInternal->size[0] = 1;
4791 iobj_12->NameInternal->size[1] = 14;
4792 cartes_emxEnsureCapacity_char_T(iobj_12->NameInternal, localB->b_kstr_o,
4793 localB);
4794 for (localB->b_kstr_o = 0; localB->b_kstr_o < 14; localB->b_kstr_o++) {
4795 iobj_12->NameInternal->data[localB->b_kstr_o] = tmp_6[localB->b_kstr_o];
4796 }
4797
4798 localB->b_kstr_o = iobj_12->Type->size[0] * iobj_12->Type->size[1];
4799 iobj_12->Type->size[0] = 1;
4800 iobj_12->Type->size[1] = 5;
4801 cartes_emxEnsureCapacity_char_T(iobj_12->Type, localB->b_kstr_o, localB);
4802 for (localB->b_kstr_o = 0; localB->b_kstr_o < 5; localB->b_kstr_o++) {
4803 iobj_12->Type->data[localB->b_kstr_o] = tmp_2[localB->b_kstr_o];
4804 }
4805
4806 localB->b_kstr_o = switch_expression->size[0] * switch_expression->size[1];
4807 switch_expression->size[0] = 1;
4808 switch_expression->size[1] = iobj_12->Type->size[1];
4809 cartes_emxEnsureCapacity_char_T(switch_expression, localB->b_kstr_o, localB);
4810 localB->loop_ub_c = iobj_12->Type->size[0] * iobj_12->Type->size[1] - 1;
4811 for (localB->b_kstr_o = 0; localB->b_kstr_o <= localB->loop_ub_c;
4812 localB->b_kstr_o++) {
4813 switch_expression->data[localB->b_kstr_o] = iobj_12->Type->data
4814 [localB->b_kstr_o];
4815 }
4816
4817 localB->b_bool_kb = false;
4818 if (switch_expression->size[1] == 8) {
4819 localB->b_kstr_o = 1;
4820 do {
4821 exitg1 = 0;
4822 if (localB->b_kstr_o - 1 < 8) {
4823 localB->loop_ub_c = localB->b_kstr_o - 1;
4824 if (switch_expression->data[localB->loop_ub_c] != localB->b_p
4825 [localB->loop_ub_c]) {
4826 exitg1 = 1;
4827 } else {
4828 localB->b_kstr_o++;
4829 }
4830 } else {
4831 localB->b_bool_kb = true;
4832 exitg1 = 1;
4833 }
4834 } while (exitg1 == 0);
4835 }
4836
4837 if (localB->b_bool_kb) {
4838 localB->b_kstr_o = 0;
4839 } else {
4840 for (localB->b_kstr_o = 0; localB->b_kstr_o < 9; localB->b_kstr_o++) {
4841 localB->b_m[localB->b_kstr_o] = tmp_4[localB->b_kstr_o];
4842 }
4843
4844 localB->b_bool_kb = false;
4845 if (switch_expression->size[1] == 9) {
4846 localB->b_kstr_o = 1;
4847 do {
4848 exitg1 = 0;
4849 if (localB->b_kstr_o - 1 < 9) {
4850 localB->loop_ub_c = localB->b_kstr_o - 1;
4851 if (switch_expression->data[localB->loop_ub_c] != localB->b_m
4852 [localB->loop_ub_c]) {
4853 exitg1 = 1;
4854 } else {
4855 localB->b_kstr_o++;
4856 }
4857 } else {
4858 localB->b_bool_kb = true;
4859 exitg1 = 1;
4860 }
4861 } while (exitg1 == 0);
4862 }
4863
4864 if (localB->b_bool_kb) {
4865 localB->b_kstr_o = 1;
4866 } else {
4867 localB->b_kstr_o = -1;
4868 }
4869 }
4870
4871 switch (localB->b_kstr_o) {
4872 case 0:
4873 localB->iv1[0] = 0;
4874 localB->iv1[1] = 0;
4875 localB->iv1[2] = 1;
4876 localB->iv1[3] = 0;
4877 localB->iv1[4] = 0;
4878 localB->iv1[5] = 0;
4879 for (localB->b_kstr_o = 0; localB->b_kstr_o < 6; localB->b_kstr_o++) {
4880 localB->msubspace_data_a[localB->b_kstr_o] = localB->iv1[localB->b_kstr_o];
4881 }
4882
4883 localB->poslim_data_c[0] = -3.1415926535897931;
4884 localB->poslim_data_c[1] = 3.1415926535897931;
4885 iobj_12->VelocityNumber = 1.0;
4886 iobj_12->PositionNumber = 1.0;
4887 iobj_12->JointAxisInternal[0] = 0.0;
4888 iobj_12->JointAxisInternal[1] = 0.0;
4889 iobj_12->JointAxisInternal[2] = 1.0;
4890 break;
4891
4892 case 1:
4893 localB->iv1[0] = 0;
4894 localB->iv1[1] = 0;
4895 localB->iv1[2] = 0;
4896 localB->iv1[3] = 0;
4897 localB->iv1[4] = 0;
4898 localB->iv1[5] = 1;
4899 for (localB->b_kstr_o = 0; localB->b_kstr_o < 6; localB->b_kstr_o++) {
4900 localB->msubspace_data_a[localB->b_kstr_o] = localB->iv1[localB->b_kstr_o];
4901 }
4902
4903 localB->poslim_data_c[0] = -0.5;
4904 localB->poslim_data_c[1] = 0.5;
4905 iobj_12->VelocityNumber = 1.0;
4906 iobj_12->PositionNumber = 1.0;
4907 iobj_12->JointAxisInternal[0] = 0.0;
4908 iobj_12->JointAxisInternal[1] = 0.0;
4909 iobj_12->JointAxisInternal[2] = 1.0;
4910 break;
4911
4912 default:
4913 for (localB->b_kstr_o = 0; localB->b_kstr_o < 6; localB->b_kstr_o++) {
4914 localB->msubspace_data_a[localB->b_kstr_o] = 0;
4915 }
4916
4917 localB->poslim_data_c[0] = 0.0;
4918 localB->poslim_data_c[1] = 0.0;
4919 iobj_12->VelocityNumber = 0.0;
4920 iobj_12->PositionNumber = 0.0;
4921 iobj_12->JointAxisInternal[0] = 0.0;
4922 iobj_12->JointAxisInternal[1] = 0.0;
4923 iobj_12->JointAxisInternal[2] = 0.0;
4924 break;
4925 }
4926
4927 localB->b_kstr_o = iobj_12->MotionSubspace->size[0] * iobj_12->
4928 MotionSubspace->size[1];
4929 iobj_12->MotionSubspace->size[0] = 6;
4930 iobj_12->MotionSubspace->size[1] = 1;
4931 cartes_emxEnsureCapacity_real_T(iobj_12->MotionSubspace, localB->b_kstr_o,
4932 localB);
4933 for (localB->b_kstr_o = 0; localB->b_kstr_o < 6; localB->b_kstr_o++) {
4934 iobj_12->MotionSubspace->data[localB->b_kstr_o] = localB->
4935 msubspace_data_a[localB->b_kstr_o];
4936 }
4937
4938 localB->b_kstr_o = iobj_12->PositionLimitsInternal->size[0] *
4939 iobj_12->PositionLimitsInternal->size[1];
4940 iobj_12->PositionLimitsInternal->size[0] = 1;
4941 iobj_12->PositionLimitsInternal->size[1] = 2;
4942 cartes_emxEnsureCapacity_real_T(iobj_12->PositionLimitsInternal,
4943 localB->b_kstr_o, localB);
4944 for (localB->b_kstr_o = 0; localB->b_kstr_o < 2; localB->b_kstr_o++) {
4945 iobj_12->PositionLimitsInternal->data[localB->b_kstr_o] =
4946 localB->poslim_data_c[localB->b_kstr_o];
4947 }
4948
4949 localB->b_kstr_o = iobj_12->HomePositionInternal->size[0];
4950 iobj_12->HomePositionInternal->size[0] = 1;
4951 cartes_emxEnsureCapacity_real_T(iobj_12->HomePositionInternal,
4952 localB->b_kstr_o, localB);
4953 for (localB->b_kstr_o = 0; localB->b_kstr_o < 1; localB->b_kstr_o++) {
4954 iobj_12->HomePositionInternal->data[0] = 0.0;
4955 }
4956
4957 iobj_5->JointInternal = iobj_12;
4958 iobj_5->Index = -1.0;
4959 iobj_5->ParentIndex = -1.0;
4960 obj->Bodies[6] = iobj_5;
4961 localB->b_kstr_o = iobj_6->NameInternal->size[0] * iobj_6->NameInternal->size
4962 [1];
4963 iobj_6->NameInternal->size[0] = 1;
4964 iobj_6->NameInternal->size[1] = 10;
4965 cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal, localB->b_kstr_o, localB);
4966 for (localB->b_kstr_o = 0; localB->b_kstr_o < 10; localB->b_kstr_o++) {
4967 iobj_6->NameInternal->data[localB->b_kstr_o] = tmp_7[localB->b_kstr_o];
4968 }
4969
4970 iobj_13->InTree = false;
4971 for (localB->b_kstr_o = 0; localB->b_kstr_o < 16; localB->b_kstr_o++) {
4972 iobj_13->JointToParentTransform[localB->b_kstr_o] = tmp_0[localB->b_kstr_o];
4973 }
4974
4975 for (localB->b_kstr_o = 0; localB->b_kstr_o < 16; localB->b_kstr_o++) {
4976 iobj_13->ChildToJointTransform[localB->b_kstr_o] = tmp_0[localB->b_kstr_o];
4977 }
4978
4979 localB->b_kstr_o = iobj_13->NameInternal->size[0] * iobj_13->
4980 NameInternal->size[1];
4981 iobj_13->NameInternal->size[0] = 1;
4982 iobj_13->NameInternal->size[1] = 14;
4983 cartes_emxEnsureCapacity_char_T(iobj_13->NameInternal, localB->b_kstr_o,
4984 localB);
4985 for (localB->b_kstr_o = 0; localB->b_kstr_o < 14; localB->b_kstr_o++) {
4986 iobj_13->NameInternal->data[localB->b_kstr_o] = tmp_8[localB->b_kstr_o];
4987 }
4988
4989 localB->b_kstr_o = iobj_13->Type->size[0] * iobj_13->Type->size[1];
4990 iobj_13->Type->size[0] = 1;
4991 iobj_13->Type->size[1] = 5;
4992 cartes_emxEnsureCapacity_char_T(iobj_13->Type, localB->b_kstr_o, localB);
4993 for (localB->b_kstr_o = 0; localB->b_kstr_o < 5; localB->b_kstr_o++) {
4994 iobj_13->Type->data[localB->b_kstr_o] = tmp_2[localB->b_kstr_o];
4995 }
4996
4997 localB->b_kstr_o = switch_expression->size[0] * switch_expression->size[1];
4998 switch_expression->size[0] = 1;
4999 switch_expression->size[1] = iobj_13->Type->size[1];
5000 cartes_emxEnsureCapacity_char_T(switch_expression, localB->b_kstr_o, localB);
5001 localB->loop_ub_c = iobj_13->Type->size[0] * iobj_13->Type->size[1] - 1;
5002 for (localB->b_kstr_o = 0; localB->b_kstr_o <= localB->loop_ub_c;
5003 localB->b_kstr_o++) {
5004 switch_expression->data[localB->b_kstr_o] = iobj_13->Type->data
5005 [localB->b_kstr_o];
5006 }
5007
5008 localB->b_bool_kb = false;
5009 if (switch_expression->size[1] == 8) {
5010 localB->b_kstr_o = 1;
5011 do {
5012 exitg1 = 0;
5013 if (localB->b_kstr_o - 1 < 8) {
5014 localB->loop_ub_c = localB->b_kstr_o - 1;
5015 if (switch_expression->data[localB->loop_ub_c] != localB->b_p
5016 [localB->loop_ub_c]) {
5017 exitg1 = 1;
5018 } else {
5019 localB->b_kstr_o++;
5020 }
5021 } else {
5022 localB->b_bool_kb = true;
5023 exitg1 = 1;
5024 }
5025 } while (exitg1 == 0);
5026 }
5027
5028 if (localB->b_bool_kb) {
5029 localB->b_kstr_o = 0;
5030 } else {
5031 for (localB->b_kstr_o = 0; localB->b_kstr_o < 9; localB->b_kstr_o++) {
5032 localB->b_m[localB->b_kstr_o] = tmp_4[localB->b_kstr_o];
5033 }
5034
5035 localB->b_bool_kb = false;
5036 if (switch_expression->size[1] == 9) {
5037 localB->b_kstr_o = 1;
5038 do {
5039 exitg1 = 0;
5040 if (localB->b_kstr_o - 1 < 9) {
5041 localB->loop_ub_c = localB->b_kstr_o - 1;
5042 if (switch_expression->data[localB->loop_ub_c] != localB->b_m
5043 [localB->loop_ub_c]) {
5044 exitg1 = 1;
5045 } else {
5046 localB->b_kstr_o++;
5047 }
5048 } else {
5049 localB->b_bool_kb = true;
5050 exitg1 = 1;
5051 }
5052 } while (exitg1 == 0);
5053 }
5054
5055 if (localB->b_bool_kb) {
5056 localB->b_kstr_o = 1;
5057 } else {
5058 localB->b_kstr_o = -1;
5059 }
5060 }
5061
5062 cartesian_trajec_emxFree_char_T(&switch_expression);
5063 switch (localB->b_kstr_o) {
5064 case 0:
5065 localB->iv1[0] = 0;
5066 localB->iv1[1] = 0;
5067 localB->iv1[2] = 1;
5068 localB->iv1[3] = 0;
5069 localB->iv1[4] = 0;
5070 localB->iv1[5] = 0;
5071 for (localB->b_kstr_o = 0; localB->b_kstr_o < 6; localB->b_kstr_o++) {
5072 localB->msubspace_data_a[localB->b_kstr_o] = localB->iv1[localB->b_kstr_o];
5073 }
5074
5075 localB->poslim_data_c[0] = -3.1415926535897931;
5076 localB->poslim_data_c[1] = 3.1415926535897931;
5077 iobj_13->VelocityNumber = 1.0;
5078 iobj_13->PositionNumber = 1.0;
5079 iobj_13->JointAxisInternal[0] = 0.0;
5080 iobj_13->JointAxisInternal[1] = 0.0;
5081 iobj_13->JointAxisInternal[2] = 1.0;
5082 break;
5083
5084 case 1:
5085 localB->iv1[0] = 0;
5086 localB->iv1[1] = 0;
5087 localB->iv1[2] = 0;
5088 localB->iv1[3] = 0;
5089 localB->iv1[4] = 0;
5090 localB->iv1[5] = 1;
5091 for (localB->b_kstr_o = 0; localB->b_kstr_o < 6; localB->b_kstr_o++) {
5092 localB->msubspace_data_a[localB->b_kstr_o] = localB->iv1[localB->b_kstr_o];
5093 }
5094
5095 localB->poslim_data_c[0] = -0.5;
5096 localB->poslim_data_c[1] = 0.5;
5097 iobj_13->VelocityNumber = 1.0;
5098 iobj_13->PositionNumber = 1.0;
5099 iobj_13->JointAxisInternal[0] = 0.0;
5100 iobj_13->JointAxisInternal[1] = 0.0;
5101 iobj_13->JointAxisInternal[2] = 1.0;
5102 break;
5103
5104 default:
5105 for (localB->b_kstr_o = 0; localB->b_kstr_o < 6; localB->b_kstr_o++) {
5106 localB->msubspace_data_a[localB->b_kstr_o] = 0;
5107 }
5108
5109 localB->poslim_data_c[0] = 0.0;
5110 localB->poslim_data_c[1] = 0.0;
5111 iobj_13->VelocityNumber = 0.0;
5112 iobj_13->PositionNumber = 0.0;
5113 iobj_13->JointAxisInternal[0] = 0.0;
5114 iobj_13->JointAxisInternal[1] = 0.0;
5115 iobj_13->JointAxisInternal[2] = 0.0;
5116 break;
5117 }
5118
5119 localB->b_kstr_o = iobj_13->MotionSubspace->size[0] * iobj_13->
5120 MotionSubspace->size[1];
5121 iobj_13->MotionSubspace->size[0] = 6;
5122 iobj_13->MotionSubspace->size[1] = 1;
5123 cartes_emxEnsureCapacity_real_T(iobj_13->MotionSubspace, localB->b_kstr_o,
5124 localB);
5125 for (localB->b_kstr_o = 0; localB->b_kstr_o < 6; localB->b_kstr_o++) {
5126 iobj_13->MotionSubspace->data[localB->b_kstr_o] = localB->
5127 msubspace_data_a[localB->b_kstr_o];
5128 }
5129
5130 localB->b_kstr_o = iobj_13->PositionLimitsInternal->size[0] *
5131 iobj_13->PositionLimitsInternal->size[1];
5132 iobj_13->PositionLimitsInternal->size[0] = 1;
5133 iobj_13->PositionLimitsInternal->size[1] = 2;
5134 cartes_emxEnsureCapacity_real_T(iobj_13->PositionLimitsInternal,
5135 localB->b_kstr_o, localB);
5136 for (localB->b_kstr_o = 0; localB->b_kstr_o < 2; localB->b_kstr_o++) {
5137 iobj_13->PositionLimitsInternal->data[localB->b_kstr_o] =
5138 localB->poslim_data_c[localB->b_kstr_o];
5139 }
5140
5141 localB->b_kstr_o = iobj_13->HomePositionInternal->size[0];
5142 iobj_13->HomePositionInternal->size[0] = 1;
5143 cartes_emxEnsureCapacity_real_T(iobj_13->HomePositionInternal,
5144 localB->b_kstr_o, localB);
5145 for (localB->b_kstr_o = 0; localB->b_kstr_o < 1; localB->b_kstr_o++) {
5146 iobj_13->HomePositionInternal->data[0] = 0.0;
5147 }
5148
5149 iobj_6->JointInternal = iobj_13;
5150 iobj_6->Index = -1.0;
5151 iobj_6->ParentIndex = -1.0;
5152 obj->Bodies[7] = iobj_6;
5153 obj->NumBodies = 0.0;
5154 obj->NumNonFixedBodies = 0.0;
5155 obj->PositionNumber = 0.0;
5156 obj->VelocityNumber = 0.0;
5157 cartesian_trajectory_plann_rand(localB->unusedExpr_c, localB, localDW);
5158 for (localB->b_kstr_o = 0; localB->b_kstr_o < 8; localB->b_kstr_o++) {
5159 obj->PositionDoFMap[localB->b_kstr_o] = 0.0;
5160 }
5161
5162 for (localB->b_kstr_o = 0; localB->b_kstr_o < 8; localB->b_kstr_o++) {
5163 obj->PositionDoFMap[localB->b_kstr_o + 8] = -1.0;
5164 }
5165
5166 for (localB->b_kstr_o = 0; localB->b_kstr_o < 8; localB->b_kstr_o++) {
5167 obj->VelocityDoFMap[localB->b_kstr_o] = 0.0;
5168 }
5169
5170 for (localB->b_kstr_o = 0; localB->b_kstr_o < 8; localB->b_kstr_o++) {
5171 obj->VelocityDoFMap[localB->b_kstr_o + 8] = -1.0;
5172 }
5173}
5174
5175static x_robotics_manip_internal_Rig_T *c_RigidBodyTree_RigidBodyTree_i
5176 (x_robotics_manip_internal_Rig_T *obj, w_robotics_manip_internal_Rig_T *iobj_0,
5177 w_robotics_manip_internal_Rig_T *iobj_1, w_robotics_manip_internal_Rig_T
5178 *iobj_2, w_robotics_manip_internal_Rig_T *iobj_3,
5179 w_robotics_manip_internal_Rig_T *iobj_4, w_robotics_manip_internal_Rig_T
5180 *iobj_5, w_robotics_manip_internal_Rig_T *iobj_6,
5181 c_rigidBodyJoint_cartesian_tr_T *iobj_7, c_rigidBodyJoint_cartesian_tr_T
5182 *iobj_8, c_rigidBodyJoint_cartesian_tr_T *iobj_9,
5183 c_rigidBodyJoint_cartesian_tr_T *iobj_10, c_rigidBodyJoint_cartesian_tr_T
5184 *iobj_11, c_rigidBodyJoint_cartesian_tr_T *iobj_12,
5185 c_rigidBodyJoint_cartesian_tr_T *iobj_13, c_rigidBodyJoint_cartesian_tr_T
5186 *iobj_14, c_rigidBodyJoint_cartesian_tr_T *iobj_15,
5187 w_robotics_manip_internal_Rig_T *iobj_16, B_MATLABSystem_cartesian_traj_T
5188 *localB, DW_MATLABSystem_cartesian_tra_T *localDW)
5189{
5190 x_robotics_manip_internal_Rig_T *b_obj;
5191 emxArray_char_T_cartesian_tra_T *switch_expression;
5192 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
5193 };
5194
5195 static const char_T tmp_0[8] = { 'b', 'a', 's', 'e', '_', 'j', 'n', 't' };
5196
5197 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
5198
5199 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
5200
5201 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
5202
5203 int32_T exitg1;
5204 b_obj = obj;
5205 localB->b_kstr_k = obj->Base.NameInternal->size[0] * obj->
5206 Base.NameInternal->size[1];
5207 obj->Base.NameInternal->size[0] = 1;
5208 obj->Base.NameInternal->size[1] = 4;
5209 cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal, localB->b_kstr_k,
5210 localB);
5211 obj->Base.NameInternal->data[0] = 'b';
5212 obj->Base.NameInternal->data[1] = 'a';
5213 obj->Base.NameInternal->data[2] = 's';
5214 obj->Base.NameInternal->data[3] = 'e';
5215 iobj_15->InTree = false;
5216 for (localB->b_kstr_k = 0; localB->b_kstr_k < 16; localB->b_kstr_k++) {
5217 iobj_15->JointToParentTransform[localB->b_kstr_k] = tmp[localB->b_kstr_k];
5218 }
5219
5220 for (localB->b_kstr_k = 0; localB->b_kstr_k < 16; localB->b_kstr_k++) {
5221 iobj_15->ChildToJointTransform[localB->b_kstr_k] = tmp[localB->b_kstr_k];
5222 }
5223
5224 localB->b_kstr_k = iobj_15->NameInternal->size[0] * iobj_15->
5225 NameInternal->size[1];
5226 iobj_15->NameInternal->size[0] = 1;
5227 iobj_15->NameInternal->size[1] = 8;
5228 cartes_emxEnsureCapacity_char_T(iobj_15->NameInternal, localB->b_kstr_k,
5229 localB);
5230 for (localB->b_kstr_k = 0; localB->b_kstr_k < 8; localB->b_kstr_k++) {
5231 iobj_15->NameInternal->data[localB->b_kstr_k] = tmp_0[localB->b_kstr_k];
5232 }
5233
5234 localB->b_kstr_k = iobj_15->Type->size[0] * iobj_15->Type->size[1];
5235 iobj_15->Type->size[0] = 1;
5236 iobj_15->Type->size[1] = 5;
5237 cartes_emxEnsureCapacity_char_T(iobj_15->Type, localB->b_kstr_k, localB);
5238 for (localB->b_kstr_k = 0; localB->b_kstr_k < 5; localB->b_kstr_k++) {
5239 iobj_15->Type->data[localB->b_kstr_k] = tmp_1[localB->b_kstr_k];
5240 }
5241
5242 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
5243 localB->b_kstr_k = switch_expression->size[0] * switch_expression->size[1];
5244 switch_expression->size[0] = 1;
5245 switch_expression->size[1] = iobj_15->Type->size[1];
5246 cartes_emxEnsureCapacity_char_T(switch_expression, localB->b_kstr_k, localB);
5247 localB->loop_ub_j = iobj_15->Type->size[0] * iobj_15->Type->size[1] - 1;
5248 for (localB->b_kstr_k = 0; localB->b_kstr_k <= localB->loop_ub_j;
5249 localB->b_kstr_k++) {
5250 switch_expression->data[localB->b_kstr_k] = iobj_15->Type->data
5251 [localB->b_kstr_k];
5252 }
5253
5254 for (localB->b_kstr_k = 0; localB->b_kstr_k < 8; localB->b_kstr_k++) {
5255 localB->b_k[localB->b_kstr_k] = tmp_2[localB->b_kstr_k];
5256 }
5257
5258 localB->b_bool_j = false;
5259 if (switch_expression->size[1] == 8) {
5260 localB->b_kstr_k = 1;
5261 do {
5262 exitg1 = 0;
5263 if (localB->b_kstr_k - 1 < 8) {
5264 localB->loop_ub_j = localB->b_kstr_k - 1;
5265 if (switch_expression->data[localB->loop_ub_j] != localB->b_k
5266 [localB->loop_ub_j]) {
5267 exitg1 = 1;
5268 } else {
5269 localB->b_kstr_k++;
5270 }
5271 } else {
5272 localB->b_bool_j = true;
5273 exitg1 = 1;
5274 }
5275 } while (exitg1 == 0);
5276 }
5277
5278 if (localB->b_bool_j) {
5279 localB->b_kstr_k = 0;
5280 } else {
5281 for (localB->b_kstr_k = 0; localB->b_kstr_k < 9; localB->b_kstr_k++) {
5282 localB->b_o[localB->b_kstr_k] = tmp_3[localB->b_kstr_k];
5283 }
5284
5285 localB->b_bool_j = false;
5286 if (switch_expression->size[1] == 9) {
5287 localB->b_kstr_k = 1;
5288 do {
5289 exitg1 = 0;
5290 if (localB->b_kstr_k - 1 < 9) {
5291 localB->loop_ub_j = localB->b_kstr_k - 1;
5292 if (switch_expression->data[localB->loop_ub_j] != localB->b_o
5293 [localB->loop_ub_j]) {
5294 exitg1 = 1;
5295 } else {
5296 localB->b_kstr_k++;
5297 }
5298 } else {
5299 localB->b_bool_j = true;
5300 exitg1 = 1;
5301 }
5302 } while (exitg1 == 0);
5303 }
5304
5305 if (localB->b_bool_j) {
5306 localB->b_kstr_k = 1;
5307 } else {
5308 localB->b_kstr_k = -1;
5309 }
5310 }
5311
5312 cartesian_trajec_emxFree_char_T(&switch_expression);
5313 switch (localB->b_kstr_k) {
5314 case 0:
5315 localB->iv[0] = 0;
5316 localB->iv[1] = 0;
5317 localB->iv[2] = 1;
5318 localB->iv[3] = 0;
5319 localB->iv[4] = 0;
5320 localB->iv[5] = 0;
5321 for (localB->b_kstr_k = 0; localB->b_kstr_k < 6; localB->b_kstr_k++) {
5322 localB->msubspace_data[localB->b_kstr_k] = localB->iv[localB->b_kstr_k];
5323 }
5324
5325 localB->poslim_data[0] = -3.1415926535897931;
5326 localB->poslim_data[1] = 3.1415926535897931;
5327 iobj_15->VelocityNumber = 1.0;
5328 iobj_15->PositionNumber = 1.0;
5329 iobj_15->JointAxisInternal[0] = 0.0;
5330 iobj_15->JointAxisInternal[1] = 0.0;
5331 iobj_15->JointAxisInternal[2] = 1.0;
5332 break;
5333
5334 case 1:
5335 localB->iv[0] = 0;
5336 localB->iv[1] = 0;
5337 localB->iv[2] = 0;
5338 localB->iv[3] = 0;
5339 localB->iv[4] = 0;
5340 localB->iv[5] = 1;
5341 for (localB->b_kstr_k = 0; localB->b_kstr_k < 6; localB->b_kstr_k++) {
5342 localB->msubspace_data[localB->b_kstr_k] = localB->iv[localB->b_kstr_k];
5343 }
5344
5345 localB->poslim_data[0] = -0.5;
5346 localB->poslim_data[1] = 0.5;
5347 iobj_15->VelocityNumber = 1.0;
5348 iobj_15->PositionNumber = 1.0;
5349 iobj_15->JointAxisInternal[0] = 0.0;
5350 iobj_15->JointAxisInternal[1] = 0.0;
5351 iobj_15->JointAxisInternal[2] = 1.0;
5352 break;
5353
5354 default:
5355 for (localB->b_kstr_k = 0; localB->b_kstr_k < 6; localB->b_kstr_k++) {
5356 localB->msubspace_data[localB->b_kstr_k] = 0;
5357 }
5358
5359 localB->poslim_data[0] = 0.0;
5360 localB->poslim_data[1] = 0.0;
5361 iobj_15->VelocityNumber = 0.0;
5362 iobj_15->PositionNumber = 0.0;
5363 iobj_15->JointAxisInternal[0] = 0.0;
5364 iobj_15->JointAxisInternal[1] = 0.0;
5365 iobj_15->JointAxisInternal[2] = 0.0;
5366 break;
5367 }
5368
5369 localB->b_kstr_k = iobj_15->MotionSubspace->size[0] * iobj_15->
5370 MotionSubspace->size[1];
5371 iobj_15->MotionSubspace->size[0] = 6;
5372 iobj_15->MotionSubspace->size[1] = 1;
5373 cartes_emxEnsureCapacity_real_T(iobj_15->MotionSubspace, localB->b_kstr_k,
5374 localB);
5375 for (localB->b_kstr_k = 0; localB->b_kstr_k < 6; localB->b_kstr_k++) {
5376 iobj_15->MotionSubspace->data[localB->b_kstr_k] = localB->
5377 msubspace_data[localB->b_kstr_k];
5378 }
5379
5380 localB->b_kstr_k = iobj_15->PositionLimitsInternal->size[0] *
5381 iobj_15->PositionLimitsInternal->size[1];
5382 iobj_15->PositionLimitsInternal->size[0] = 1;
5383 iobj_15->PositionLimitsInternal->size[1] = 2;
5384 cartes_emxEnsureCapacity_real_T(iobj_15->PositionLimitsInternal,
5385 localB->b_kstr_k, localB);
5386 for (localB->b_kstr_k = 0; localB->b_kstr_k < 2; localB->b_kstr_k++) {
5387 iobj_15->PositionLimitsInternal->data[localB->b_kstr_k] =
5388 localB->poslim_data[localB->b_kstr_k];
5389 }
5390
5391 localB->b_kstr_k = iobj_15->HomePositionInternal->size[0];
5392 iobj_15->HomePositionInternal->size[0] = 1;
5393 cartes_emxEnsureCapacity_real_T(iobj_15->HomePositionInternal,
5394 localB->b_kstr_k, localB);
5395 for (localB->b_kstr_k = 0; localB->b_kstr_k < 1; localB->b_kstr_k++) {
5396 iobj_15->HomePositionInternal->data[0] = 0.0;
5397 }
5398
5399 obj->Base.JointInternal = iobj_15;
5400 obj->Base.Index = -1.0;
5401 obj->Base.ParentIndex = -1.0;
5402 obj->Base.Index = 0.0;
5403 cartesian_trajectory_plann_rand(localB->unusedExpr, localB, localDW);
5404 ca_RigidBodyTree_clearAllBodies(obj, iobj_0, iobj_1, iobj_2, iobj_3, iobj_4,
5405 iobj_5, iobj_6, iobj_8, iobj_9, iobj_10, iobj_11, iobj_12, iobj_13, iobj_14,
5406 iobj_7, iobj_16, localB, localDW);
5407 return b_obj;
5408}
5409
5410static boolean_T cartesian_trajectory_pla_strcmp(const
5411 emxArray_char_T_cartesian_tra_T *a, const emxArray_char_T_cartesian_tra_T *b)
5412{
5413 boolean_T b_bool;
5414 int32_T kstr;
5415 int32_T b_kstr;
5416 boolean_T d;
5417 int32_T exitg1;
5418 b_bool = false;
5419 d = (a->size[1] == 0);
5420 if (d && (b->size[1] == 0)) {
5421 b_bool = true;
5422 } else if (a->size[1] != b->size[1]) {
5423 } else {
5424 b_kstr = 1;
5425 do {
5426 exitg1 = 0;
5427 if (b_kstr - 1 <= b->size[1] - 1) {
5428 kstr = b_kstr - 1;
5429 if (a->data[kstr] != b->data[kstr]) {
5430 exitg1 = 1;
5431 } else {
5432 b_kstr++;
5433 }
5434 } else {
5435 b_bool = true;
5436 exitg1 = 1;
5437 }
5438 } while (exitg1 == 0);
5439 }
5440
5441 return b_bool;
5442}
5443
5444static c_rigidBodyJoint_cartesian_tr_T *c_rigidBodyJoint_rigidBodyJoint
5445 (c_rigidBodyJoint_cartesian_tr_T *obj, const emxArray_char_T_cartesian_tra_T
5446 *jname, const emxArray_char_T_cartesian_tra_T *jtype,
5447 B_MATLABSystem_cartesian_traj_T *localB)
5448{
5449 c_rigidBodyJoint_cartesian_tr_T *b_obj;
5450 emxArray_char_T_cartesian_tra_T *switch_expression;
5451 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
5452 };
5453
5454 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
5455
5456 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
5457
5458 static const char_T tmp_2[5] = { 'f', 'i', 'x', 'e', 'd' };
5459
5460 static const char_T tmp_3[128] = { '\x00', '\x01', '\x02', '\x03', '\x04',
5461 '\x05', '\x06', '\x07', '\x08', '\x09', '\x0a', '\x0b', '\x0c', '\x0d',
5462 '\x0e', '\x0f', '\x10', '\x11', '\x12', '\x13', '\x14', '\x15', '\x16',
5463 '\x17', '\x18', '\x19', '\x1a', '\x1b', '\x1c', '\x1d', '\x1e', '\x1f', ' ',
5464 '!', '\"', '#', '$', '%', '&', '\'', '(', ')', '*', '+', ',', '-', '.', '/',
5465 '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', ':', ';', '<', '=', '>',
5466 '?', '@', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm',
5467 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z', '[', '\\',
5468 ']', '^', '_', '`', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k',
5469 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z',
5470 '{', '|', '}', '~', '\x7f' };
5471
5472 boolean_T guard1 = false;
5473 boolean_T guard2 = false;
5474 boolean_T guard3 = false;
5475 boolean_T guard4 = false;
5476 int32_T exitg1;
5477 boolean_T guard11 = false;
5478 obj->InTree = false;
5479 for (localB->minnanb = 0; localB->minnanb < 16; localB->minnanb++) {
5480 obj->JointToParentTransform[localB->minnanb] = tmp[localB->minnanb];
5481 }
5482
5483 for (localB->minnanb = 0; localB->minnanb < 16; localB->minnanb++) {
5484 obj->ChildToJointTransform[localB->minnanb] = tmp[localB->minnanb];
5485 }
5486
5487 b_obj = obj;
5488 localB->minnanb = obj->NameInternal->size[0] * obj->NameInternal->size[1];
5489 obj->NameInternal->size[0] = 1;
5490 obj->NameInternal->size[1] = jname->size[1];
5491 cartes_emxEnsureCapacity_char_T(obj->NameInternal, localB->minnanb, localB);
5492 localB->loop_ub_dr = jname->size[0] * jname->size[1] - 1;
5493 for (localB->minnanb = 0; localB->minnanb <= localB->loop_ub_dr;
5494 localB->minnanb++) {
5495 obj->NameInternal->data[localB->minnanb] = jname->data[localB->minnanb];
5496 }
5497
5498 localB->partial_match_size_idx_1 = 0;
5499 for (localB->minnanb = 0; localB->minnanb < 8; localB->minnanb++) {
5500 localB->vstr[localB->minnanb] = tmp_0[localB->minnanb];
5501 }
5502
5503 guard1 = false;
5504 guard2 = false;
5505 guard3 = false;
5506 guard4 = false;
5507 if (jtype->size[1] <= 8) {
5508 localB->loop_ub_dr = jtype->size[1];
5509 for (localB->minnanb = 0; localB->minnanb < 8; localB->minnanb++) {
5510 localB->b_n[localB->minnanb] = tmp_0[localB->minnanb];
5511 }
5512
5513 localB->b_bool_e = false;
5514 localB->minnanb = jtype->size[1];
5515 if (localB->minnanb >= 8) {
5516 localB->minnanb = 8;
5517 }
5518
5519 guard11 = false;
5520 if (localB->loop_ub_dr <= localB->minnanb) {
5521 if (localB->minnanb < localB->loop_ub_dr) {
5522 localB->loop_ub_dr = localB->minnanb;
5523 }
5524
5525 localB->minnanb = localB->loop_ub_dr - 1;
5526 guard11 = true;
5527 } else {
5528 if (jtype->size[1] == 8) {
5529 localB->minnanb = 7;
5530 guard11 = true;
5531 }
5532 }
5533
5534 if (guard11) {
5535 localB->loop_ub_dr = 1;
5536 do {
5537 exitg1 = 0;
5538 if (localB->loop_ub_dr - 1 <= localB->minnanb) {
5539 localB->kstr_k = localB->loop_ub_dr - 1;
5540 if (tmp_3[static_cast<uint8_T>(jtype->data[localB->kstr_k]) & 127] !=
5541 tmp_3[static_cast<int32_T>(localB->b_n[localB->kstr_k])]) {
5542 exitg1 = 1;
5543 } else {
5544 localB->loop_ub_dr++;
5545 }
5546 } else {
5547 localB->b_bool_e = true;
5548 exitg1 = 1;
5549 }
5550 } while (exitg1 == 0);
5551 }
5552
5553 if (localB->b_bool_e) {
5554 if (jtype->size[1] == 8) {
5555 localB->nmatched = 1;
5556 localB->partial_match_size_idx_1 = 8;
5557 for (localB->minnanb = 0; localB->minnanb < 8; localB->minnanb++) {
5558 localB->b_f[localB->minnanb] = localB->vstr[localB->minnanb];
5559 }
5560 } else {
5561 localB->partial_match_size_idx_1 = 8;
5562 for (localB->minnanb = 0; localB->minnanb < 8; localB->minnanb++) {
5563 localB->partial_match_data[localB->minnanb] = localB->vstr
5564 [localB->minnanb];
5565 }
5566
5567 localB->matched = true;
5568 localB->nmatched = 1;
5569 guard3 = true;
5570 }
5571 } else {
5572 guard4 = true;
5573 }
5574 } else {
5575 guard4 = true;
5576 }
5577
5578 if (guard4) {
5579 localB->matched = false;
5580 localB->nmatched = 0;
5581 guard3 = true;
5582 }
5583
5584 if (guard3) {
5585 for (localB->minnanb = 0; localB->minnanb < 9; localB->minnanb++) {
5586 localB->b_vstr[localB->minnanb] = tmp_1[localB->minnanb];
5587 }
5588
5589 if (jtype->size[1] <= 9) {
5590 localB->loop_ub_dr = jtype->size[1];
5591 for (localB->minnanb = 0; localB->minnanb < 9; localB->minnanb++) {
5592 localB->b_f[localB->minnanb] = tmp_1[localB->minnanb];
5593 }
5594
5595 localB->b_bool_e = false;
5596 localB->minnanb = jtype->size[1];
5597 if (localB->minnanb >= 9) {
5598 localB->minnanb = 9;
5599 }
5600
5601 guard11 = false;
5602 if (localB->loop_ub_dr <= localB->minnanb) {
5603 if (localB->minnanb < localB->loop_ub_dr) {
5604 localB->loop_ub_dr = localB->minnanb;
5605 }
5606
5607 localB->minnanb = localB->loop_ub_dr - 1;
5608 guard11 = true;
5609 } else {
5610 if (jtype->size[1] == 9) {
5611 localB->minnanb = 8;
5612 guard11 = true;
5613 }
5614 }
5615
5616 if (guard11) {
5617 localB->loop_ub_dr = 1;
5618 do {
5619 exitg1 = 0;
5620 if (localB->loop_ub_dr - 1 <= localB->minnanb) {
5621 localB->kstr_k = localB->loop_ub_dr - 1;
5622 if (tmp_3[static_cast<uint8_T>(jtype->data[localB->kstr_k]) & 127]
5623 != tmp_3[static_cast<int32_T>(localB->b_f[localB->kstr_k])]) {
5624 exitg1 = 1;
5625 } else {
5626 localB->loop_ub_dr++;
5627 }
5628 } else {
5629 localB->b_bool_e = true;
5630 exitg1 = 1;
5631 }
5632 } while (exitg1 == 0);
5633 }
5634
5635 if (localB->b_bool_e) {
5636 if (jtype->size[1] == 9) {
5637 localB->nmatched = 1;
5638 localB->partial_match_size_idx_1 = 9;
5639 for (localB->minnanb = 0; localB->minnanb < 9; localB->minnanb++) {
5640 localB->b_f[localB->minnanb] = localB->b_vstr[localB->minnanb];
5641 }
5642 } else {
5643 if (!localB->matched) {
5644 localB->partial_match_size_idx_1 = 9;
5645 for (localB->minnanb = 0; localB->minnanb < 9; localB->minnanb++) {
5646 localB->partial_match_data[localB->minnanb] = localB->
5647 b_vstr[localB->minnanb];
5648 }
5649 }
5650
5651 localB->matched = true;
5652 localB->nmatched++;
5653 guard2 = true;
5654 }
5655 } else {
5656 guard2 = true;
5657 }
5658 } else {
5659 guard2 = true;
5660 }
5661 }
5662
5663 if (guard2) {
5664 for (localB->minnanb = 0; localB->minnanb < 5; localB->minnanb++) {
5665 localB->c_vstr[localB->minnanb] = tmp_2[localB->minnanb];
5666 }
5667
5668 if (jtype->size[1] <= 5) {
5669 localB->loop_ub_dr = jtype->size[1];
5670 for (localB->minnanb = 0; localB->minnanb < 5; localB->minnanb++) {
5671 localB->b_l[localB->minnanb] = tmp_2[localB->minnanb];
5672 }
5673
5674 localB->b_bool_e = false;
5675 localB->minnanb = jtype->size[1];
5676 if (localB->minnanb >= 5) {
5677 localB->minnanb = 5;
5678 }
5679
5680 guard11 = false;
5681 if (localB->loop_ub_dr <= localB->minnanb) {
5682 if (localB->minnanb < localB->loop_ub_dr) {
5683 localB->loop_ub_dr = localB->minnanb;
5684 }
5685
5686 localB->minnanb = localB->loop_ub_dr - 1;
5687 guard11 = true;
5688 } else {
5689 if (jtype->size[1] == 5) {
5690 localB->minnanb = 4;
5691 guard11 = true;
5692 }
5693 }
5694
5695 if (guard11) {
5696 localB->loop_ub_dr = 1;
5697 do {
5698 exitg1 = 0;
5699 if (localB->loop_ub_dr - 1 <= localB->minnanb) {
5700 localB->kstr_k = localB->loop_ub_dr - 1;
5701 if (tmp_3[static_cast<uint8_T>(jtype->data[localB->kstr_k]) & 127]
5702 != tmp_3[static_cast<int32_T>(localB->b_l[localB->kstr_k])]) {
5703 exitg1 = 1;
5704 } else {
5705 localB->loop_ub_dr++;
5706 }
5707 } else {
5708 localB->b_bool_e = true;
5709 exitg1 = 1;
5710 }
5711 } while (exitg1 == 0);
5712 }
5713
5714 if (localB->b_bool_e) {
5715 if (jtype->size[1] == 5) {
5716 localB->nmatched = 1;
5717 localB->partial_match_size_idx_1 = 5;
5718 for (localB->minnanb = 0; localB->minnanb < 5; localB->minnanb++) {
5719 localB->b_f[localB->minnanb] = localB->c_vstr[localB->minnanb];
5720 }
5721 } else {
5722 if (!localB->matched) {
5723 localB->partial_match_size_idx_1 = 5;
5724 for (localB->minnanb = 0; localB->minnanb < 5; localB->minnanb++) {
5725 localB->partial_match_data[localB->minnanb] = localB->
5726 c_vstr[localB->minnanb];
5727 }
5728 }
5729
5730 localB->nmatched++;
5731 guard1 = true;
5732 }
5733 } else {
5734 guard1 = true;
5735 }
5736 } else {
5737 guard1 = true;
5738 }
5739 }
5740
5741 if (guard1) {
5742 if (localB->nmatched == 0) {
5743 localB->partial_match_size_idx_1 = 0;
5744 } else {
5745 localB->loop_ub_dr = localB->partial_match_size_idx_1 - 1;
5746 if (0 <= localB->loop_ub_dr) {
5747 memcpy(&localB->b_f[0], &localB->partial_match_data[0],
5748 (localB->loop_ub_dr + 1) * sizeof(char_T));
5749 }
5750 }
5751 }
5752
5753 if ((localB->nmatched == 0) || ((jtype->size[1] == 0) !=
5754 (localB->partial_match_size_idx_1 == 0))) {
5755 localB->partial_match_size_idx_1 = 0;
5756 } else {
5757 localB->loop_ub_dr = localB->partial_match_size_idx_1 - 1;
5758 if (0 <= localB->loop_ub_dr) {
5759 memcpy(&localB->partial_match_data[0], &localB->b_f[0],
5760 (localB->loop_ub_dr + 1) * sizeof(char_T));
5761 }
5762 }
5763
5764 localB->minnanb = obj->Type->size[0] * obj->Type->size[1];
5765 obj->Type->size[0] = 1;
5766 obj->Type->size[1] = localB->partial_match_size_idx_1;
5767 cartes_emxEnsureCapacity_char_T(obj->Type, localB->minnanb, localB);
5768 localB->loop_ub_dr = localB->partial_match_size_idx_1 - 1;
5769 for (localB->minnanb = 0; localB->minnanb <= localB->loop_ub_dr;
5770 localB->minnanb++) {
5771 obj->Type->data[localB->minnanb] = localB->partial_match_data
5772 [localB->minnanb];
5773 }
5774
5775 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
5776 localB->minnanb = switch_expression->size[0] * switch_expression->size[1];
5777 switch_expression->size[0] = 1;
5778 switch_expression->size[1] = obj->Type->size[1];
5779 cartes_emxEnsureCapacity_char_T(switch_expression, localB->minnanb, localB);
5780 localB->loop_ub_dr = obj->Type->size[0] * obj->Type->size[1] - 1;
5781 for (localB->minnanb = 0; localB->minnanb <= localB->loop_ub_dr;
5782 localB->minnanb++) {
5783 switch_expression->data[localB->minnanb] = obj->Type->data[localB->minnanb];
5784 }
5785
5786 for (localB->minnanb = 0; localB->minnanb < 8; localB->minnanb++) {
5787 localB->b_n[localB->minnanb] = tmp_0[localB->minnanb];
5788 }
5789
5790 localB->b_bool_e = false;
5791 if (switch_expression->size[1] == 8) {
5792 localB->loop_ub_dr = 1;
5793 do {
5794 exitg1 = 0;
5795 if (localB->loop_ub_dr - 1 < 8) {
5796 localB->kstr_k = localB->loop_ub_dr - 1;
5797 if (switch_expression->data[localB->kstr_k] != localB->b_n
5798 [localB->kstr_k]) {
5799 exitg1 = 1;
5800 } else {
5801 localB->loop_ub_dr++;
5802 }
5803 } else {
5804 localB->b_bool_e = true;
5805 exitg1 = 1;
5806 }
5807 } while (exitg1 == 0);
5808 }
5809
5810 if (localB->b_bool_e) {
5811 localB->minnanb = 0;
5812 } else {
5813 for (localB->minnanb = 0; localB->minnanb < 9; localB->minnanb++) {
5814 localB->b_f[localB->minnanb] = tmp_1[localB->minnanb];
5815 }
5816
5817 localB->b_bool_e = false;
5818 if (switch_expression->size[1] == 9) {
5819 localB->loop_ub_dr = 1;
5820 do {
5821 exitg1 = 0;
5822 if (localB->loop_ub_dr - 1 < 9) {
5823 localB->kstr_k = localB->loop_ub_dr - 1;
5824 if (switch_expression->data[localB->kstr_k] != localB->b_f
5825 [localB->kstr_k]) {
5826 exitg1 = 1;
5827 } else {
5828 localB->loop_ub_dr++;
5829 }
5830 } else {
5831 localB->b_bool_e = true;
5832 exitg1 = 1;
5833 }
5834 } while (exitg1 == 0);
5835 }
5836
5837 if (localB->b_bool_e) {
5838 localB->minnanb = 1;
5839 } else {
5840 localB->minnanb = -1;
5841 }
5842 }
5843
5844 cartesian_trajec_emxFree_char_T(&switch_expression);
5845 switch (localB->minnanb) {
5846 case 0:
5847 localB->iv3[0] = 0;
5848 localB->iv3[1] = 0;
5849 localB->iv3[2] = 1;
5850 localB->iv3[3] = 0;
5851 localB->iv3[4] = 0;
5852 localB->iv3[5] = 0;
5853 for (localB->minnanb = 0; localB->minnanb < 6; localB->minnanb++) {
5854 localB->msubspace_data_af[localB->minnanb] = localB->iv3[localB->minnanb];
5855 }
5856
5857 localB->poslim_data_p[0] = -3.1415926535897931;
5858 localB->poslim_data_p[1] = 3.1415926535897931;
5859 obj->VelocityNumber = 1.0;
5860 obj->PositionNumber = 1.0;
5861 obj->JointAxisInternal[0] = 0.0;
5862 obj->JointAxisInternal[1] = 0.0;
5863 obj->JointAxisInternal[2] = 1.0;
5864 break;
5865
5866 case 1:
5867 localB->iv3[0] = 0;
5868 localB->iv3[1] = 0;
5869 localB->iv3[2] = 0;
5870 localB->iv3[3] = 0;
5871 localB->iv3[4] = 0;
5872 localB->iv3[5] = 1;
5873 for (localB->minnanb = 0; localB->minnanb < 6; localB->minnanb++) {
5874 localB->msubspace_data_af[localB->minnanb] = localB->iv3[localB->minnanb];
5875 }
5876
5877 localB->poslim_data_p[0] = -0.5;
5878 localB->poslim_data_p[1] = 0.5;
5879 obj->VelocityNumber = 1.0;
5880 obj->PositionNumber = 1.0;
5881 obj->JointAxisInternal[0] = 0.0;
5882 obj->JointAxisInternal[1] = 0.0;
5883 obj->JointAxisInternal[2] = 1.0;
5884 break;
5885
5886 default:
5887 for (localB->minnanb = 0; localB->minnanb < 6; localB->minnanb++) {
5888 localB->msubspace_data_af[localB->minnanb] = 0;
5889 }
5890
5891 localB->poslim_data_p[0] = 0.0;
5892 localB->poslim_data_p[1] = 0.0;
5893 obj->VelocityNumber = 0.0;
5894 obj->PositionNumber = 0.0;
5895 obj->JointAxisInternal[0] = 0.0;
5896 obj->JointAxisInternal[1] = 0.0;
5897 obj->JointAxisInternal[2] = 0.0;
5898 break;
5899 }
5900
5901 localB->minnanb = obj->MotionSubspace->size[0] * obj->MotionSubspace->size[1];
5902 obj->MotionSubspace->size[0] = 6;
5903 obj->MotionSubspace->size[1] = 1;
5904 cartes_emxEnsureCapacity_real_T(obj->MotionSubspace, localB->minnanb, localB);
5905 for (localB->minnanb = 0; localB->minnanb < 6; localB->minnanb++) {
5906 obj->MotionSubspace->data[localB->minnanb] = localB->
5907 msubspace_data_af[localB->minnanb];
5908 }
5909
5910 localB->minnanb = obj->PositionLimitsInternal->size[0] *
5911 obj->PositionLimitsInternal->size[1];
5912 obj->PositionLimitsInternal->size[0] = 1;
5913 obj->PositionLimitsInternal->size[1] = 2;
5914 cartes_emxEnsureCapacity_real_T(obj->PositionLimitsInternal, localB->minnanb,
5915 localB);
5916 for (localB->minnanb = 0; localB->minnanb < 2; localB->minnanb++) {
5917 obj->PositionLimitsInternal->data[localB->minnanb] = localB->
5918 poslim_data_p[localB->minnanb];
5919 }
5920
5921 localB->minnanb = obj->HomePositionInternal->size[0];
5922 obj->HomePositionInternal->size[0] = 1;
5923 cartes_emxEnsureCapacity_real_T(obj->HomePositionInternal, localB->minnanb,
5924 localB);
5925 for (localB->minnanb = 0; localB->minnanb < 1; localB->minnanb++) {
5926 obj->HomePositionInternal->data[0] = 0.0;
5927 }
5928
5929 return b_obj;
5930}
5931
5932static void cartesian_trajec_emxFree_real_T(emxArray_real_T_cartesian_tra_T
5933 **pEmxArray)
5934{
5935 if (*pEmxArray != (emxArray_real_T_cartesian_tra_T *)NULL) {
5936 if (((*pEmxArray)->data != (real_T *)NULL) && (*pEmxArray)->canFreeData) {
5937 free((*pEmxArray)->data);
5938 }
5939
5940 free((*pEmxArray)->size);
5941 free(*pEmxArray);
5942 *pEmxArray = (emxArray_real_T_cartesian_tra_T *)NULL;
5943 }
5944}
5945
5946static w_robotics_manip_internal_Rig_T *cartesian_trajec_RigidBody_copy(const
5947 v_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_tr_T *iobj_0,
5948 c_rigidBodyJoint_cartesian_tr_T *iobj_1, w_robotics_manip_internal_Rig_T
5949 *iobj_2, B_MATLABSystem_cartesian_traj_T *localB)
5950{
5951 w_robotics_manip_internal_Rig_T *newbody;
5952 c_rigidBodyJoint_cartesian_tr_T *newjoint;
5953 emxArray_char_T_cartesian_tra_T *jtype;
5954 emxArray_char_T_cartesian_tra_T *jname;
5955 emxArray_real_T_cartesian_tra_T *obj_0;
5956 emxArray_real_T_cartesian_tra_T *obj_1;
5957 emxArray_real_T_cartesian_tra_T *obj_2;
5958 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
5959 };
5960
5961 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
5962
5963 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
5964
5965 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
5966
5967 int32_T exitg1;
5968 cartesian_trajec_emxInit_char_T(&jtype, 2, localB);
5969 localB->b_kstr_jz = jtype->size[0] * jtype->size[1];
5970 jtype->size[0] = 1;
5971 jtype->size[1] = obj->NameInternal->size[1];
5972 cartes_emxEnsureCapacity_char_T(jtype, localB->b_kstr_jz, localB);
5973 localB->loop_ub_l = obj->NameInternal->size[0] * obj->NameInternal->size[1] -
5974 1;
5975 for (localB->b_kstr_jz = 0; localB->b_kstr_jz <= localB->loop_ub_l;
5976 localB->b_kstr_jz++) {
5977 jtype->data[localB->b_kstr_jz] = obj->NameInternal->data[localB->b_kstr_jz];
5978 }
5979
5980 newbody = iobj_2;
5981 localB->b_kstr_jz = iobj_2->NameInternal->size[0] * iobj_2->NameInternal->
5982 size[1];
5983 iobj_2->NameInternal->size[0] = 1;
5984 iobj_2->NameInternal->size[1] = jtype->size[1];
5985 cartes_emxEnsureCapacity_char_T(iobj_2->NameInternal, localB->b_kstr_jz,
5986 localB);
5987 localB->loop_ub_l = jtype->size[0] * jtype->size[1] - 1;
5988 for (localB->b_kstr_jz = 0; localB->b_kstr_jz <= localB->loop_ub_l;
5989 localB->b_kstr_jz++) {
5990 iobj_2->NameInternal->data[localB->b_kstr_jz] = jtype->data
5991 [localB->b_kstr_jz];
5992 }
5993
5994 iobj_0->InTree = false;
5995 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 16; localB->b_kstr_jz++) {
5996 iobj_0->JointToParentTransform[localB->b_kstr_jz] = tmp[localB->b_kstr_jz];
5997 }
5998
5999 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 16; localB->b_kstr_jz++) {
6000 iobj_0->ChildToJointTransform[localB->b_kstr_jz] = tmp[localB->b_kstr_jz];
6001 }
6002
6003 localB->b_kstr_jz = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->
6004 size[1];
6005 iobj_0->NameInternal->size[0] = 1;
6006 iobj_0->NameInternal->size[1] = jtype->size[1] + 4;
6007 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, localB->b_kstr_jz,
6008 localB);
6009 localB->loop_ub_l = jtype->size[0] * jtype->size[1];
6010 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < localB->loop_ub_l;
6011 localB->b_kstr_jz++) {
6012 iobj_0->NameInternal->data[localB->b_kstr_jz] = jtype->data
6013 [localB->b_kstr_jz];
6014 }
6015
6016 iobj_0->NameInternal->data[localB->loop_ub_l] = '_';
6017 iobj_0->NameInternal->data[localB->loop_ub_l + 1] = 'j';
6018 iobj_0->NameInternal->data[localB->loop_ub_l + 2] = 'n';
6019 iobj_0->NameInternal->data[localB->loop_ub_l + 3] = 't';
6020 localB->b_kstr_jz = iobj_0->Type->size[0] * iobj_0->Type->size[1];
6021 iobj_0->Type->size[0] = 1;
6022 iobj_0->Type->size[1] = 5;
6023 cartes_emxEnsureCapacity_char_T(iobj_0->Type, localB->b_kstr_jz, localB);
6024 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 5; localB->b_kstr_jz++) {
6025 iobj_0->Type->data[localB->b_kstr_jz] = tmp_0[localB->b_kstr_jz];
6026 }
6027
6028 localB->b_kstr_jz = jtype->size[0] * jtype->size[1];
6029 jtype->size[0] = 1;
6030 jtype->size[1] = iobj_0->Type->size[1];
6031 cartes_emxEnsureCapacity_char_T(jtype, localB->b_kstr_jz, localB);
6032 localB->loop_ub_l = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
6033 for (localB->b_kstr_jz = 0; localB->b_kstr_jz <= localB->loop_ub_l;
6034 localB->b_kstr_jz++) {
6035 jtype->data[localB->b_kstr_jz] = iobj_0->Type->data[localB->b_kstr_jz];
6036 }
6037
6038 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 8; localB->b_kstr_jz++) {
6039 localB->b_ch[localB->b_kstr_jz] = tmp_1[localB->b_kstr_jz];
6040 }
6041
6042 localB->b_bool_g = false;
6043 if (jtype->size[1] == 8) {
6044 localB->b_kstr_jz = 1;
6045 do {
6046 exitg1 = 0;
6047 if (localB->b_kstr_jz - 1 < 8) {
6048 localB->loop_ub_l = localB->b_kstr_jz - 1;
6049 if (jtype->data[localB->loop_ub_l] != localB->b_ch[localB->loop_ub_l]) {
6050 exitg1 = 1;
6051 } else {
6052 localB->b_kstr_jz++;
6053 }
6054 } else {
6055 localB->b_bool_g = true;
6056 exitg1 = 1;
6057 }
6058 } while (exitg1 == 0);
6059 }
6060
6061 if (localB->b_bool_g) {
6062 localB->b_kstr_jz = 0;
6063 } else {
6064 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 9; localB->b_kstr_jz++) {
6065 localB->b_c[localB->b_kstr_jz] = tmp_2[localB->b_kstr_jz];
6066 }
6067
6068 localB->b_bool_g = false;
6069 if (jtype->size[1] == 9) {
6070 localB->b_kstr_jz = 1;
6071 do {
6072 exitg1 = 0;
6073 if (localB->b_kstr_jz - 1 < 9) {
6074 localB->loop_ub_l = localB->b_kstr_jz - 1;
6075 if (jtype->data[localB->loop_ub_l] != localB->b_c[localB->loop_ub_l])
6076 {
6077 exitg1 = 1;
6078 } else {
6079 localB->b_kstr_jz++;
6080 }
6081 } else {
6082 localB->b_bool_g = true;
6083 exitg1 = 1;
6084 }
6085 } while (exitg1 == 0);
6086 }
6087
6088 if (localB->b_bool_g) {
6089 localB->b_kstr_jz = 1;
6090 } else {
6091 localB->b_kstr_jz = -1;
6092 }
6093 }
6094
6095 switch (localB->b_kstr_jz) {
6096 case 0:
6097 localB->iv2[0] = 0;
6098 localB->iv2[1] = 0;
6099 localB->iv2[2] = 1;
6100 localB->iv2[3] = 0;
6101 localB->iv2[4] = 0;
6102 localB->iv2[5] = 0;
6103 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 6; localB->b_kstr_jz++) {
6104 localB->msubspace_data_d[localB->b_kstr_jz] = localB->iv2
6105 [localB->b_kstr_jz];
6106 }
6107
6108 localB->poslim_data_ct[0] = -3.1415926535897931;
6109 localB->poslim_data_ct[1] = 3.1415926535897931;
6110 iobj_0->VelocityNumber = 1.0;
6111 iobj_0->PositionNumber = 1.0;
6112 iobj_0->JointAxisInternal[0] = 0.0;
6113 iobj_0->JointAxisInternal[1] = 0.0;
6114 iobj_0->JointAxisInternal[2] = 1.0;
6115 break;
6116
6117 case 1:
6118 localB->iv2[0] = 0;
6119 localB->iv2[1] = 0;
6120 localB->iv2[2] = 0;
6121 localB->iv2[3] = 0;
6122 localB->iv2[4] = 0;
6123 localB->iv2[5] = 1;
6124 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 6; localB->b_kstr_jz++) {
6125 localB->msubspace_data_d[localB->b_kstr_jz] = localB->iv2
6126 [localB->b_kstr_jz];
6127 }
6128
6129 localB->poslim_data_ct[0] = -0.5;
6130 localB->poslim_data_ct[1] = 0.5;
6131 iobj_0->VelocityNumber = 1.0;
6132 iobj_0->PositionNumber = 1.0;
6133 iobj_0->JointAxisInternal[0] = 0.0;
6134 iobj_0->JointAxisInternal[1] = 0.0;
6135 iobj_0->JointAxisInternal[2] = 1.0;
6136 break;
6137
6138 default:
6139 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 6; localB->b_kstr_jz++) {
6140 localB->msubspace_data_d[localB->b_kstr_jz] = 0;
6141 }
6142
6143 localB->poslim_data_ct[0] = 0.0;
6144 localB->poslim_data_ct[1] = 0.0;
6145 iobj_0->VelocityNumber = 0.0;
6146 iobj_0->PositionNumber = 0.0;
6147 iobj_0->JointAxisInternal[0] = 0.0;
6148 iobj_0->JointAxisInternal[1] = 0.0;
6149 iobj_0->JointAxisInternal[2] = 0.0;
6150 break;
6151 }
6152
6153 localB->b_kstr_jz = iobj_0->MotionSubspace->size[0] * iobj_0->
6154 MotionSubspace->size[1];
6155 iobj_0->MotionSubspace->size[0] = 6;
6156 iobj_0->MotionSubspace->size[1] = 1;
6157 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, localB->b_kstr_jz,
6158 localB);
6159 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 6; localB->b_kstr_jz++) {
6160 iobj_0->MotionSubspace->data[localB->b_kstr_jz] = localB->
6161 msubspace_data_d[localB->b_kstr_jz];
6162 }
6163
6164 localB->b_kstr_jz = iobj_0->PositionLimitsInternal->size[0] *
6165 iobj_0->PositionLimitsInternal->size[1];
6166 iobj_0->PositionLimitsInternal->size[0] = 1;
6167 iobj_0->PositionLimitsInternal->size[1] = 2;
6168 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal,
6169 localB->b_kstr_jz, localB);
6170 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 2; localB->b_kstr_jz++) {
6171 iobj_0->PositionLimitsInternal->data[localB->b_kstr_jz] =
6172 localB->poslim_data_ct[localB->b_kstr_jz];
6173 }
6174
6175 localB->b_kstr_jz = iobj_0->HomePositionInternal->size[0];
6176 iobj_0->HomePositionInternal->size[0] = 1;
6177 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal,
6178 localB->b_kstr_jz, localB);
6179 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 1; localB->b_kstr_jz++) {
6180 iobj_0->HomePositionInternal->data[0] = 0.0;
6181 }
6182
6183 iobj_2->JointInternal = iobj_0;
6184 iobj_2->Index = -1.0;
6185 iobj_2->ParentIndex = -1.0;
6186 localB->b_kstr_jz = jtype->size[0] * jtype->size[1];
6187 jtype->size[0] = 1;
6188 jtype->size[1] = obj->JointInternal.Type->size[1];
6189 cartes_emxEnsureCapacity_char_T(jtype, localB->b_kstr_jz, localB);
6190 localB->loop_ub_l = obj->JointInternal.Type->size[0] * obj->
6191 JointInternal.Type->size[1] - 1;
6192 for (localB->b_kstr_jz = 0; localB->b_kstr_jz <= localB->loop_ub_l;
6193 localB->b_kstr_jz++) {
6194 jtype->data[localB->b_kstr_jz] = obj->JointInternal.Type->data
6195 [localB->b_kstr_jz];
6196 }
6197
6198 cartesian_trajec_emxInit_char_T(&jname, 2, localB);
6199 localB->b_kstr_jz = jname->size[0] * jname->size[1];
6200 jname->size[0] = 1;
6201 jname->size[1] = obj->JointInternal.NameInternal->size[1];
6202 cartes_emxEnsureCapacity_char_T(jname, localB->b_kstr_jz, localB);
6203 localB->loop_ub_l = obj->JointInternal.NameInternal->size[0] *
6204 obj->JointInternal.NameInternal->size[1] - 1;
6205 for (localB->b_kstr_jz = 0; localB->b_kstr_jz <= localB->loop_ub_l;
6206 localB->b_kstr_jz++) {
6207 jname->data[localB->b_kstr_jz] = obj->JointInternal.NameInternal->
6208 data[localB->b_kstr_jz];
6209 }
6210
6211 newjoint = c_rigidBodyJoint_rigidBodyJoint(iobj_1, jname, jtype, localB);
6212 localB->b_kstr_jz = jtype->size[0] * jtype->size[1];
6213 jtype->size[0] = 1;
6214 jtype->size[1] = obj->JointInternal.NameInternal->size[1];
6215 cartes_emxEnsureCapacity_char_T(jtype, localB->b_kstr_jz, localB);
6216 localB->loop_ub_l = obj->JointInternal.NameInternal->size[0] *
6217 obj->JointInternal.NameInternal->size[1] - 1;
6218 cartesian_trajec_emxFree_char_T(&jname);
6219 for (localB->b_kstr_jz = 0; localB->b_kstr_jz <= localB->loop_ub_l;
6220 localB->b_kstr_jz++) {
6221 jtype->data[localB->b_kstr_jz] = obj->JointInternal.NameInternal->
6222 data[localB->b_kstr_jz];
6223 }
6224
6225 if (jtype->size[1] != 0) {
6226 localB->b_kstr_jz = jtype->size[0] * jtype->size[1];
6227 jtype->size[0] = 1;
6228 jtype->size[1] = obj->JointInternal.NameInternal->size[1];
6229 cartes_emxEnsureCapacity_char_T(jtype, localB->b_kstr_jz, localB);
6230 localB->loop_ub_l = obj->JointInternal.NameInternal->size[0] *
6231 obj->JointInternal.NameInternal->size[1] - 1;
6232 for (localB->b_kstr_jz = 0; localB->b_kstr_jz <= localB->loop_ub_l;
6233 localB->b_kstr_jz++) {
6234 jtype->data[localB->b_kstr_jz] = obj->JointInternal.NameInternal->
6235 data[localB->b_kstr_jz];
6236 }
6237
6238 if (!newjoint->InTree) {
6239 localB->b_kstr_jz = newjoint->NameInternal->size[0] *
6240 newjoint->NameInternal->size[1];
6241 newjoint->NameInternal->size[0] = 1;
6242 newjoint->NameInternal->size[1] = jtype->size[1];
6243 cartes_emxEnsureCapacity_char_T(newjoint->NameInternal, localB->b_kstr_jz,
6244 localB);
6245 localB->loop_ub_l = jtype->size[0] * jtype->size[1] - 1;
6246 for (localB->b_kstr_jz = 0; localB->b_kstr_jz <= localB->loop_ub_l;
6247 localB->b_kstr_jz++) {
6248 newjoint->NameInternal->data[localB->b_kstr_jz] = jtype->data
6249 [localB->b_kstr_jz];
6250 }
6251 }
6252 }
6253
6254 cartesian_trajec_emxFree_char_T(&jtype);
6255 cartesian_trajec_emxInit_real_T(&obj_0, 1, localB);
6256 localB->loop_ub_l = obj->JointInternal.PositionLimitsInternal->size[0] *
6257 obj->JointInternal.PositionLimitsInternal->size[1];
6258 localB->b_kstr_jz = newjoint->PositionLimitsInternal->size[0] *
6259 newjoint->PositionLimitsInternal->size[1];
6260 newjoint->PositionLimitsInternal->size[0] =
6261 obj->JointInternal.PositionLimitsInternal->size[0];
6262 newjoint->PositionLimitsInternal->size[1] = 2;
6263 cartes_emxEnsureCapacity_real_T(newjoint->PositionLimitsInternal,
6264 localB->b_kstr_jz, localB);
6265 localB->b_kstr_jz = obj_0->size[0];
6266 obj_0->size[0] = localB->loop_ub_l;
6267 cartes_emxEnsureCapacity_real_T(obj_0, localB->b_kstr_jz, localB);
6268 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < localB->loop_ub_l;
6269 localB->b_kstr_jz++) {
6270 obj_0->data[localB->b_kstr_jz] = obj->
6271 JointInternal.PositionLimitsInternal->data[localB->b_kstr_jz];
6272 }
6273
6274 localB->loop_ub_l = obj_0->size[0];
6275 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < localB->loop_ub_l;
6276 localB->b_kstr_jz++) {
6277 newjoint->PositionLimitsInternal->data[localB->b_kstr_jz] = obj_0->
6278 data[localB->b_kstr_jz];
6279 }
6280
6281 cartesian_trajec_emxFree_real_T(&obj_0);
6282 cartesian_trajec_emxInit_real_T(&obj_1, 1, localB);
6283 localB->b_kstr_jz = obj_1->size[0];
6284 obj_1->size[0] = obj->JointInternal.HomePositionInternal->size[0];
6285 cartes_emxEnsureCapacity_real_T(obj_1, localB->b_kstr_jz, localB);
6286 localB->loop_ub_l = obj->JointInternal.HomePositionInternal->size[0];
6287 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < localB->loop_ub_l;
6288 localB->b_kstr_jz++) {
6289 obj_1->data[localB->b_kstr_jz] = obj->
6290 JointInternal.HomePositionInternal->data[localB->b_kstr_jz];
6291 }
6292
6293 localB->b_kstr_jz = newjoint->HomePositionInternal->size[0];
6294 newjoint->HomePositionInternal->size[0] = obj_1->size[0];
6295 cartes_emxEnsureCapacity_real_T(newjoint->HomePositionInternal,
6296 localB->b_kstr_jz, localB);
6297 localB->loop_ub_l = obj_1->size[0];
6298 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < localB->loop_ub_l;
6299 localB->b_kstr_jz++) {
6300 newjoint->HomePositionInternal->data[localB->b_kstr_jz] = obj_1->data
6301 [localB->b_kstr_jz];
6302 }
6303
6304 cartesian_trajec_emxFree_real_T(&obj_1);
6305 localB->obj_idx_0 = obj->JointInternal.JointAxisInternal[0];
6306 localB->obj_idx_1 = obj->JointInternal.JointAxisInternal[1];
6307 localB->obj_idx_2 = obj->JointInternal.JointAxisInternal[2];
6308 newjoint->JointAxisInternal[0] = localB->obj_idx_0;
6309 newjoint->JointAxisInternal[1] = localB->obj_idx_1;
6310 newjoint->JointAxisInternal[2] = localB->obj_idx_2;
6311 cartesian_trajec_emxInit_real_T(&obj_2, 1, localB);
6312 localB->loop_ub_l = obj->JointInternal.MotionSubspace->size[0] *
6313 obj->JointInternal.MotionSubspace->size[1];
6314 localB->b_kstr_jz = newjoint->MotionSubspace->size[0] *
6315 newjoint->MotionSubspace->size[1];
6316 newjoint->MotionSubspace->size[0] = 6;
6317 newjoint->MotionSubspace->size[1] = obj->JointInternal.MotionSubspace->size[1];
6318 cartes_emxEnsureCapacity_real_T(newjoint->MotionSubspace, localB->b_kstr_jz,
6319 localB);
6320 localB->b_kstr_jz = obj_2->size[0];
6321 obj_2->size[0] = localB->loop_ub_l;
6322 cartes_emxEnsureCapacity_real_T(obj_2, localB->b_kstr_jz, localB);
6323 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < localB->loop_ub_l;
6324 localB->b_kstr_jz++) {
6325 obj_2->data[localB->b_kstr_jz] = obj->JointInternal.MotionSubspace->
6326 data[localB->b_kstr_jz];
6327 }
6328
6329 localB->loop_ub_l = obj_2->size[0];
6330 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < localB->loop_ub_l;
6331 localB->b_kstr_jz++) {
6332 newjoint->MotionSubspace->data[localB->b_kstr_jz] = obj_2->data
6333 [localB->b_kstr_jz];
6334 }
6335
6336 cartesian_trajec_emxFree_real_T(&obj_2);
6337 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 16; localB->b_kstr_jz++) {
6338 localB->obj[localB->b_kstr_jz] = obj->
6339 JointInternal.JointToParentTransform[localB->b_kstr_jz];
6340 }
6341
6342 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 16; localB->b_kstr_jz++) {
6343 newjoint->JointToParentTransform[localB->b_kstr_jz] = localB->obj
6344 [localB->b_kstr_jz];
6345 }
6346
6347 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 16; localB->b_kstr_jz++) {
6348 localB->obj[localB->b_kstr_jz] = obj->
6349 JointInternal.ChildToJointTransform[localB->b_kstr_jz];
6350 }
6351
6352 for (localB->b_kstr_jz = 0; localB->b_kstr_jz < 16; localB->b_kstr_jz++) {
6353 newjoint->ChildToJointTransform[localB->b_kstr_jz] = localB->obj
6354 [localB->b_kstr_jz];
6355 }
6356
6357 iobj_2->JointInternal = newjoint;
6358 return newbody;
6359}
6360
6361static void cartesian_RigidBodyTree_addBody(x_robotics_manip_internal_Rig_T *obj,
6362 v_robotics_manip_internal_Rig_T *bodyin, const emxArray_char_T_cartesian_tra_T
6363 *parentName, c_rigidBodyJoint_cartesian_tr_T *iobj_0,
6364 c_rigidBodyJoint_cartesian_tr_T *iobj_1, w_robotics_manip_internal_Rig_T
6365 *iobj_2, B_MATLABSystem_cartesian_traj_T *localB)
6366{
6367 w_robotics_manip_internal_Rig_T *body;
6368 c_rigidBodyJoint_cartesian_tr_T *jnt;
6369 emxArray_char_T_cartesian_tra_T *bname;
6370 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
6371
6372 boolean_T exitg1;
6373 int32_T exitg2;
6374 cartesian_trajec_emxInit_char_T(&bname, 2, localB);
6375 localB->pid = -1.0;
6376 localB->b_kstr_hn = bname->size[0] * bname->size[1];
6377 bname->size[0] = 1;
6378 bname->size[1] = obj->Base.NameInternal->size[1];
6379 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_hn, localB);
6380 localB->loop_ub_g3 = obj->Base.NameInternal->size[0] * obj->
6381 Base.NameInternal->size[1] - 1;
6382 for (localB->b_kstr_hn = 0; localB->b_kstr_hn <= localB->loop_ub_g3;
6383 localB->b_kstr_hn++) {
6384 bname->data[localB->b_kstr_hn] = obj->Base.NameInternal->data
6385 [localB->b_kstr_hn];
6386 }
6387
6388 if (cartesian_trajectory_pla_strcmp(bname, parentName)) {
6389 localB->pid = 0.0;
6390 } else {
6391 localB->b_index_e = obj->NumBodies;
6392 localB->b_i_j3 = 0;
6393 exitg1 = false;
6394 while ((!exitg1) && (localB->b_i_j3 <= static_cast<int32_T>
6395 (localB->b_index_e) - 1)) {
6396 body = obj->Bodies[localB->b_i_j3];
6397 localB->b_kstr_hn = bname->size[0] * bname->size[1];
6398 bname->size[0] = 1;
6399 bname->size[1] = body->NameInternal->size[1];
6400 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_hn, localB);
6401 localB->loop_ub_g3 = body->NameInternal->size[0] * body->
6402 NameInternal->size[1] - 1;
6403 for (localB->b_kstr_hn = 0; localB->b_kstr_hn <= localB->loop_ub_g3;
6404 localB->b_kstr_hn++) {
6405 bname->data[localB->b_kstr_hn] = body->NameInternal->data
6406 [localB->b_kstr_hn];
6407 }
6408
6409 if (cartesian_trajectory_pla_strcmp(bname, parentName)) {
6410 localB->pid = static_cast<real_T>(localB->b_i_j3) + 1.0;
6411 exitg1 = true;
6412 } else {
6413 localB->b_i_j3++;
6414 }
6415 }
6416 }
6417
6418 localB->b_index_e = obj->NumBodies + 1.0;
6419 body = cartesian_trajec_RigidBody_copy(bodyin, iobj_1, iobj_0, iobj_2, localB);
6420 obj->Bodies[static_cast<int32_T>(localB->b_index_e) - 1] = body;
6421 body->Index = localB->b_index_e;
6422 body->ParentIndex = localB->pid;
6423 body->JointInternal->InTree = true;
6424 obj->NumBodies++;
6425 jnt = body->JointInternal;
6426 localB->b_kstr_hn = bname->size[0] * bname->size[1];
6427 bname->size[0] = 1;
6428 bname->size[1] = jnt->Type->size[1];
6429 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_hn, localB);
6430 localB->loop_ub_g3 = jnt->Type->size[0] * jnt->Type->size[1] - 1;
6431 for (localB->b_kstr_hn = 0; localB->b_kstr_hn <= localB->loop_ub_g3;
6432 localB->b_kstr_hn++) {
6433 bname->data[localB->b_kstr_hn] = jnt->Type->data[localB->b_kstr_hn];
6434 }
6435
6436 for (localB->b_kstr_hn = 0; localB->b_kstr_hn < 5; localB->b_kstr_hn++) {
6437 localB->b_ob[localB->b_kstr_hn] = tmp[localB->b_kstr_hn];
6438 }
6439
6440 localB->b_bool_cg = false;
6441 if (bname->size[1] == 5) {
6442 localB->b_kstr_hn = 1;
6443 do {
6444 exitg2 = 0;
6445 if (localB->b_kstr_hn - 1 < 5) {
6446 localB->loop_ub_g3 = localB->b_kstr_hn - 1;
6447 if (bname->data[localB->loop_ub_g3] != localB->b_ob[localB->loop_ub_g3])
6448 {
6449 exitg2 = 1;
6450 } else {
6451 localB->b_kstr_hn++;
6452 }
6453 } else {
6454 localB->b_bool_cg = true;
6455 exitg2 = 1;
6456 }
6457 } while (exitg2 == 0);
6458 }
6459
6460 cartesian_trajec_emxFree_char_T(&bname);
6461 if (!localB->b_bool_cg) {
6462 obj->NumNonFixedBodies++;
6463 jnt = body->JointInternal;
6464 localB->b_kstr_hn = static_cast<int32_T>(body->Index) - 1;
6465 obj->PositionDoFMap[localB->b_kstr_hn] = obj->PositionNumber + 1.0;
6466 obj->PositionDoFMap[localB->b_kstr_hn + 8] = obj->PositionNumber +
6467 jnt->PositionNumber;
6468 jnt = body->JointInternal;
6469 localB->b_kstr_hn = static_cast<int32_T>(body->Index) - 1;
6470 obj->VelocityDoFMap[localB->b_kstr_hn] = obj->VelocityNumber + 1.0;
6471 obj->VelocityDoFMap[localB->b_kstr_hn + 8] = obj->VelocityNumber +
6472 jnt->VelocityNumber;
6473 } else {
6474 localB->b_kstr_hn = static_cast<int32_T>(body->Index);
6475 obj->PositionDoFMap[localB->b_kstr_hn - 1] = 0.0;
6476 obj->PositionDoFMap[localB->b_kstr_hn + 7] = -1.0;
6477 localB->b_kstr_hn = static_cast<int32_T>(body->Index);
6478 obj->VelocityDoFMap[localB->b_kstr_hn - 1] = 0.0;
6479 obj->VelocityDoFMap[localB->b_kstr_hn + 7] = -1.0;
6480 }
6481
6482 jnt = body->JointInternal;
6483 obj->PositionNumber += jnt->PositionNumber;
6484 jnt = body->JointInternal;
6485 obj->VelocityNumber += jnt->VelocityNumber;
6486}
6487
6488static void inverseKinematics_set_RigidBody(b_inverseKinematics_cartesian_T *obj,
6489 y_robotics_manip_internal_Rig_T *rigidbodytree,
6490 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
6491 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
6492 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
6493 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
6494 w_robotics_manip_internal_Rig_T *iobj_6, w_robotics_manip_internal_Rig_T
6495 *iobj_7, w_robotics_manip_internal_Rig_T *iobj_8,
6496 w_robotics_manip_internal_Rig_T *iobj_9, w_robotics_manip_internal_Rig_T
6497 *iobj_10, w_robotics_manip_internal_Rig_T *iobj_11,
6498 w_robotics_manip_internal_Rig_T *iobj_12, w_robotics_manip_internal_Rig_T
6499 *iobj_13, w_robotics_manip_internal_Rig_T *iobj_14,
6500 c_rigidBodyJoint_cartesian_tr_T *iobj_15, c_rigidBodyJoint_cartesian_tr_T
6501 *iobj_16, c_rigidBodyJoint_cartesian_tr_T *iobj_17,
6502 c_rigidBodyJoint_cartesian_tr_T *iobj_18, c_rigidBodyJoint_cartesian_tr_T
6503 *iobj_19, c_rigidBodyJoint_cartesian_tr_T *iobj_20,
6504 c_rigidBodyJoint_cartesian_tr_T *iobj_21, c_rigidBodyJoint_cartesian_tr_T
6505 *iobj_22, c_rigidBodyJoint_cartesian_tr_T *iobj_23,
6506 c_rigidBodyJoint_cartesian_tr_T *iobj_24, c_rigidBodyJoint_cartesian_tr_T
6507 *iobj_25, c_rigidBodyJoint_cartesian_tr_T *iobj_26,
6508 c_rigidBodyJoint_cartesian_tr_T *iobj_27, c_rigidBodyJoint_cartesian_tr_T
6509 *iobj_28, c_rigidBodyJoint_cartesian_tr_T *iobj_29,
6510 c_rigidBodyJoint_cartesian_tr_T *iobj_30, c_rigidBodyJoint_cartesian_tr_T
6511 *iobj_31, c_rigidBodyJoint_cartesian_tr_T *iobj_32,
6512 c_rigidBodyJoint_cartesian_tr_T *iobj_33, c_rigidBodyJoint_cartesian_tr_T
6513 *iobj_34, c_rigidBodyJoint_cartesian_tr_T *iobj_35,
6514 c_rigidBodyJoint_cartesian_tr_T *iobj_36, c_rigidBodyJoint_cartesian_tr_T
6515 *iobj_37, c_rigidBodyJoint_cartesian_tr_T *iobj_38,
6516 c_rigidBodyJoint_cartesian_tr_T *iobj_39, w_robotics_manip_internal_Rig_T
6517 *iobj_40, x_robotics_manip_internal_Rig_T *iobj_41,
6518 B_MATLABSystem_cartesian_traj_T *localB, DW_MATLABSystem_cartesian_tra_T
6519 *localDW)
6520{
6521 x_robotics_manip_internal_Rig_T *newrobot;
6522 v_robotics_manip_internal_Rig_T *body;
6523 v_robotics_manip_internal_Rig_T *parent;
6524 emxArray_char_T_cartesian_tra_T *b_basename;
6525 w_robotics_manip_internal_Rig_T *body_0;
6526 c_rigidBodyJoint_cartesian_tr_T *jnt;
6527 emxArray_char_T_cartesian_tra_T *bname;
6528 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
6529
6530 boolean_T exitg1;
6531 int32_T exitg2;
6532 cartesian_trajec_emxInit_char_T(&b_basename, 2, localB);
6533 newrobot = c_RigidBodyTree_RigidBodyTree_i(iobj_41, iobj_0, iobj_1, iobj_2,
6534 iobj_3, iobj_4, iobj_5, iobj_6, iobj_15, iobj_16, iobj_17, iobj_18, iobj_19,
6535 iobj_20, iobj_21, iobj_22, iobj_39, iobj_40, localB, localDW);
6536 localB->b_kstr_e = b_basename->size[0] * b_basename->size[1];
6537 b_basename->size[0] = 1;
6538 b_basename->size[1] = rigidbodytree->Base.NameInternal->size[1];
6539 cartes_emxEnsureCapacity_char_T(b_basename, localB->b_kstr_e, localB);
6540 localB->loop_ub_h = rigidbodytree->Base.NameInternal->size[0] *
6541 rigidbodytree->Base.NameInternal->size[1] - 1;
6542 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6543 localB->b_kstr_e++) {
6544 b_basename->data[localB->b_kstr_e] = rigidbodytree->Base.NameInternal->
6545 data[localB->b_kstr_e];
6546 }
6547
6548 cartesian_trajec_emxInit_char_T(&bname, 2, localB);
6549 localB->bid_m = -1.0;
6550 localB->b_kstr_e = bname->size[0] * bname->size[1];
6551 bname->size[0] = 1;
6552 bname->size[1] = newrobot->Base.NameInternal->size[1];
6553 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_e, localB);
6554 localB->loop_ub_h = newrobot->Base.NameInternal->size[0] *
6555 newrobot->Base.NameInternal->size[1] - 1;
6556 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6557 localB->b_kstr_e++) {
6558 bname->data[localB->b_kstr_e] = newrobot->Base.NameInternal->data
6559 [localB->b_kstr_e];
6560 }
6561
6562 if (cartesian_trajectory_pla_strcmp(bname, b_basename)) {
6563 localB->bid_m = 0.0;
6564 } else {
6565 localB->b_index = newrobot->NumBodies;
6566 localB->b_i_h = 0;
6567 exitg1 = false;
6568 while ((!exitg1) && (localB->b_i_h <= static_cast<int32_T>(localB->b_index)
6569 - 1)) {
6570 body_0 = newrobot->Bodies[localB->b_i_h];
6571 localB->b_kstr_e = bname->size[0] * bname->size[1];
6572 bname->size[0] = 1;
6573 bname->size[1] = body_0->NameInternal->size[1];
6574 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_e, localB);
6575 localB->loop_ub_h = body_0->NameInternal->size[0] * body_0->
6576 NameInternal->size[1] - 1;
6577 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6578 localB->b_kstr_e++) {
6579 bname->data[localB->b_kstr_e] = body_0->NameInternal->data
6580 [localB->b_kstr_e];
6581 }
6582
6583 if (cartesian_trajectory_pla_strcmp(bname, b_basename)) {
6584 localB->bid_m = static_cast<real_T>(localB->b_i_h) + 1.0;
6585 exitg1 = true;
6586 } else {
6587 localB->b_i_h++;
6588 }
6589 }
6590 }
6591
6592 if ((!(localB->bid_m == 0.0)) && (localB->bid_m < 0.0)) {
6593 localB->b_kstr_e = newrobot->Base.NameInternal->size[0] *
6594 newrobot->Base.NameInternal->size[1];
6595 newrobot->Base.NameInternal->size[0] = 1;
6596 newrobot->Base.NameInternal->size[1] = b_basename->size[1];
6597 cartes_emxEnsureCapacity_char_T(newrobot->Base.NameInternal,
6598 localB->b_kstr_e, localB);
6599 localB->loop_ub_h = b_basename->size[0] * b_basename->size[1] - 1;
6600 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6601 localB->b_kstr_e++) {
6602 newrobot->Base.NameInternal->data[localB->b_kstr_e] = b_basename->
6603 data[localB->b_kstr_e];
6604 }
6605 }
6606
6607 if (1.0 <= rigidbodytree->NumBodies) {
6608 body = rigidbodytree->Bodies[0];
6609 localB->bid_m = body->ParentIndex;
6610 if (localB->bid_m > 0.0) {
6611 parent = rigidbodytree->Bodies[static_cast<int32_T>(localB->bid_m) - 1];
6612 } else {
6613 parent = &rigidbodytree->Base;
6614 }
6615
6616 localB->b_kstr_e = bname->size[0] * bname->size[1];
6617 bname->size[0] = 1;
6618 bname->size[1] = parent->NameInternal->size[1];
6619 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_e, localB);
6620 localB->loop_ub_h = parent->NameInternal->size[0] * parent->
6621 NameInternal->size[1] - 1;
6622 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6623 localB->b_kstr_e++) {
6624 bname->data[localB->b_kstr_e] = parent->NameInternal->data
6625 [localB->b_kstr_e];
6626 }
6627
6628 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_24, iobj_23,
6629 iobj_7, localB);
6630 }
6631
6632 if (2.0 <= rigidbodytree->NumBodies) {
6633 body = rigidbodytree->Bodies[1];
6634 localB->bid_m = body->ParentIndex;
6635 if (localB->bid_m > 0.0) {
6636 parent = rigidbodytree->Bodies[static_cast<int32_T>(localB->bid_m) - 1];
6637 } else {
6638 parent = &rigidbodytree->Base;
6639 }
6640
6641 localB->b_kstr_e = bname->size[0] * bname->size[1];
6642 bname->size[0] = 1;
6643 bname->size[1] = parent->NameInternal->size[1];
6644 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_e, localB);
6645 localB->loop_ub_h = parent->NameInternal->size[0] * parent->
6646 NameInternal->size[1] - 1;
6647 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6648 localB->b_kstr_e++) {
6649 bname->data[localB->b_kstr_e] = parent->NameInternal->data
6650 [localB->b_kstr_e];
6651 }
6652
6653 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_26, iobj_25,
6654 iobj_8, localB);
6655 }
6656
6657 if (3.0 <= rigidbodytree->NumBodies) {
6658 body = rigidbodytree->Bodies[2];
6659 localB->bid_m = body->ParentIndex;
6660 if (localB->bid_m > 0.0) {
6661 parent = rigidbodytree->Bodies[static_cast<int32_T>(localB->bid_m) - 1];
6662 } else {
6663 parent = &rigidbodytree->Base;
6664 }
6665
6666 localB->b_kstr_e = bname->size[0] * bname->size[1];
6667 bname->size[0] = 1;
6668 bname->size[1] = parent->NameInternal->size[1];
6669 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_e, localB);
6670 localB->loop_ub_h = parent->NameInternal->size[0] * parent->
6671 NameInternal->size[1] - 1;
6672 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6673 localB->b_kstr_e++) {
6674 bname->data[localB->b_kstr_e] = parent->NameInternal->data
6675 [localB->b_kstr_e];
6676 }
6677
6678 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_28, iobj_27,
6679 iobj_9, localB);
6680 }
6681
6682 if (4.0 <= rigidbodytree->NumBodies) {
6683 body = rigidbodytree->Bodies[3];
6684 localB->bid_m = body->ParentIndex;
6685 if (localB->bid_m > 0.0) {
6686 parent = rigidbodytree->Bodies[static_cast<int32_T>(localB->bid_m) - 1];
6687 } else {
6688 parent = &rigidbodytree->Base;
6689 }
6690
6691 localB->b_kstr_e = bname->size[0] * bname->size[1];
6692 bname->size[0] = 1;
6693 bname->size[1] = parent->NameInternal->size[1];
6694 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_e, localB);
6695 localB->loop_ub_h = parent->NameInternal->size[0] * parent->
6696 NameInternal->size[1] - 1;
6697 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6698 localB->b_kstr_e++) {
6699 bname->data[localB->b_kstr_e] = parent->NameInternal->data
6700 [localB->b_kstr_e];
6701 }
6702
6703 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_30, iobj_29,
6704 iobj_10, localB);
6705 }
6706
6707 if (5.0 <= rigidbodytree->NumBodies) {
6708 body = rigidbodytree->Bodies[4];
6709 localB->bid_m = body->ParentIndex;
6710 if (localB->bid_m > 0.0) {
6711 parent = rigidbodytree->Bodies[static_cast<int32_T>(localB->bid_m) - 1];
6712 } else {
6713 parent = &rigidbodytree->Base;
6714 }
6715
6716 localB->b_kstr_e = bname->size[0] * bname->size[1];
6717 bname->size[0] = 1;
6718 bname->size[1] = parent->NameInternal->size[1];
6719 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_e, localB);
6720 localB->loop_ub_h = parent->NameInternal->size[0] * parent->
6721 NameInternal->size[1] - 1;
6722 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6723 localB->b_kstr_e++) {
6724 bname->data[localB->b_kstr_e] = parent->NameInternal->data
6725 [localB->b_kstr_e];
6726 }
6727
6728 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_32, iobj_31,
6729 iobj_11, localB);
6730 }
6731
6732 if (6.0 <= rigidbodytree->NumBodies) {
6733 body = rigidbodytree->Bodies[5];
6734 localB->bid_m = body->ParentIndex;
6735 if (localB->bid_m > 0.0) {
6736 parent = rigidbodytree->Bodies[static_cast<int32_T>(localB->bid_m) - 1];
6737 } else {
6738 parent = &rigidbodytree->Base;
6739 }
6740
6741 localB->b_kstr_e = bname->size[0] * bname->size[1];
6742 bname->size[0] = 1;
6743 bname->size[1] = parent->NameInternal->size[1];
6744 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_e, localB);
6745 localB->loop_ub_h = parent->NameInternal->size[0] * parent->
6746 NameInternal->size[1] - 1;
6747 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6748 localB->b_kstr_e++) {
6749 bname->data[localB->b_kstr_e] = parent->NameInternal->data
6750 [localB->b_kstr_e];
6751 }
6752
6753 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_34, iobj_33,
6754 iobj_12, localB);
6755 }
6756
6757 if (7.0 <= rigidbodytree->NumBodies) {
6758 body = rigidbodytree->Bodies[6];
6759 localB->bid_m = body->ParentIndex;
6760 if (localB->bid_m > 0.0) {
6761 parent = rigidbodytree->Bodies[static_cast<int32_T>(localB->bid_m) - 1];
6762 } else {
6763 parent = &rigidbodytree->Base;
6764 }
6765
6766 localB->b_kstr_e = bname->size[0] * bname->size[1];
6767 bname->size[0] = 1;
6768 bname->size[1] = parent->NameInternal->size[1];
6769 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_e, localB);
6770 localB->loop_ub_h = parent->NameInternal->size[0] * parent->
6771 NameInternal->size[1] - 1;
6772 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6773 localB->b_kstr_e++) {
6774 bname->data[localB->b_kstr_e] = parent->NameInternal->data
6775 [localB->b_kstr_e];
6776 }
6777
6778 localB->bid_m = -1.0;
6779 localB->b_kstr_e = b_basename->size[0] * b_basename->size[1];
6780 b_basename->size[0] = 1;
6781 b_basename->size[1] = newrobot->Base.NameInternal->size[1];
6782 cartes_emxEnsureCapacity_char_T(b_basename, localB->b_kstr_e, localB);
6783 localB->loop_ub_h = newrobot->Base.NameInternal->size[0] *
6784 newrobot->Base.NameInternal->size[1] - 1;
6785 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6786 localB->b_kstr_e++) {
6787 b_basename->data[localB->b_kstr_e] = newrobot->Base.NameInternal->
6788 data[localB->b_kstr_e];
6789 }
6790
6791 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
6792 localB->bid_m = 0.0;
6793 } else {
6794 localB->b_index = newrobot->NumBodies;
6795 localB->b_i_h = 0;
6796 exitg1 = false;
6797 while ((!exitg1) && (localB->b_i_h <= static_cast<int32_T>(localB->b_index)
6798 - 1)) {
6799 body_0 = newrobot->Bodies[localB->b_i_h];
6800 localB->b_kstr_e = b_basename->size[0] * b_basename->size[1];
6801 b_basename->size[0] = 1;
6802 b_basename->size[1] = body_0->NameInternal->size[1];
6803 cartes_emxEnsureCapacity_char_T(b_basename, localB->b_kstr_e, localB);
6804 localB->loop_ub_h = body_0->NameInternal->size[0] * body_0->
6805 NameInternal->size[1] - 1;
6806 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6807 localB->b_kstr_e++) {
6808 b_basename->data[localB->b_kstr_e] = body_0->NameInternal->data
6809 [localB->b_kstr_e];
6810 }
6811
6812 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
6813 localB->bid_m = static_cast<real_T>(localB->b_i_h) + 1.0;
6814 exitg1 = true;
6815 } else {
6816 localB->b_i_h++;
6817 }
6818 }
6819 }
6820
6821 localB->b_index = newrobot->NumBodies + 1.0;
6822 body_0 = cartesian_trajec_RigidBody_copy(body, iobj_35, iobj_36, iobj_13,
6823 localB);
6824 newrobot->Bodies[static_cast<int32_T>(localB->b_index) - 1] = body_0;
6825 body_0->Index = localB->b_index;
6826 body_0->ParentIndex = localB->bid_m;
6827 body_0->JointInternal->InTree = true;
6828 newrobot->NumBodies++;
6829 jnt = body_0->JointInternal;
6830 localB->b_kstr_e = bname->size[0] * bname->size[1];
6831 bname->size[0] = 1;
6832 bname->size[1] = jnt->Type->size[1];
6833 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_e, localB);
6834 localB->loop_ub_h = jnt->Type->size[0] * jnt->Type->size[1] - 1;
6835 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6836 localB->b_kstr_e++) {
6837 bname->data[localB->b_kstr_e] = jnt->Type->data[localB->b_kstr_e];
6838 }
6839
6840 for (localB->b_kstr_e = 0; localB->b_kstr_e < 5; localB->b_kstr_e++) {
6841 localB->b_c5[localB->b_kstr_e] = tmp[localB->b_kstr_e];
6842 }
6843
6844 localB->b_bool_c = false;
6845 if (bname->size[1] == 5) {
6846 localB->b_kstr_e = 1;
6847 do {
6848 exitg2 = 0;
6849 if (localB->b_kstr_e - 1 < 5) {
6850 localB->loop_ub_h = localB->b_kstr_e - 1;
6851 if (bname->data[localB->loop_ub_h] != localB->b_c5[localB->loop_ub_h])
6852 {
6853 exitg2 = 1;
6854 } else {
6855 localB->b_kstr_e++;
6856 }
6857 } else {
6858 localB->b_bool_c = true;
6859 exitg2 = 1;
6860 }
6861 } while (exitg2 == 0);
6862 }
6863
6864 if (!localB->b_bool_c) {
6865 newrobot->NumNonFixedBodies++;
6866 jnt = body_0->JointInternal;
6867 localB->b_kstr_e = static_cast<int32_T>(body_0->Index) - 1;
6868 newrobot->PositionDoFMap[localB->b_kstr_e] = newrobot->PositionNumber +
6869 1.0;
6870 newrobot->PositionDoFMap[localB->b_kstr_e + 8] = newrobot->PositionNumber
6871 + jnt->PositionNumber;
6872 jnt = body_0->JointInternal;
6873 localB->b_kstr_e = static_cast<int32_T>(body_0->Index) - 1;
6874 newrobot->VelocityDoFMap[localB->b_kstr_e] = newrobot->VelocityNumber +
6875 1.0;
6876 newrobot->VelocityDoFMap[localB->b_kstr_e + 8] = newrobot->VelocityNumber
6877 + jnt->VelocityNumber;
6878 } else {
6879 localB->b_kstr_e = static_cast<int32_T>(body_0->Index);
6880 newrobot->PositionDoFMap[localB->b_kstr_e - 1] = 0.0;
6881 newrobot->PositionDoFMap[localB->b_kstr_e + 7] = -1.0;
6882 localB->b_kstr_e = static_cast<int32_T>(body_0->Index);
6883 newrobot->VelocityDoFMap[localB->b_kstr_e - 1] = 0.0;
6884 newrobot->VelocityDoFMap[localB->b_kstr_e + 7] = -1.0;
6885 }
6886
6887 jnt = body_0->JointInternal;
6888 newrobot->PositionNumber += jnt->PositionNumber;
6889 jnt = body_0->JointInternal;
6890 newrobot->VelocityNumber += jnt->VelocityNumber;
6891 }
6892
6893 if (8.0 <= rigidbodytree->NumBodies) {
6894 body = rigidbodytree->Bodies[7];
6895 localB->bid_m = body->ParentIndex;
6896 if (localB->bid_m > 0.0) {
6897 parent = rigidbodytree->Bodies[static_cast<int32_T>(localB->bid_m) - 1];
6898 } else {
6899 parent = &rigidbodytree->Base;
6900 }
6901
6902 localB->b_kstr_e = bname->size[0] * bname->size[1];
6903 bname->size[0] = 1;
6904 bname->size[1] = parent->NameInternal->size[1];
6905 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_e, localB);
6906 localB->loop_ub_h = parent->NameInternal->size[0] * parent->
6907 NameInternal->size[1] - 1;
6908 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6909 localB->b_kstr_e++) {
6910 bname->data[localB->b_kstr_e] = parent->NameInternal->data
6911 [localB->b_kstr_e];
6912 }
6913
6914 localB->bid_m = -1.0;
6915 localB->b_kstr_e = b_basename->size[0] * b_basename->size[1];
6916 b_basename->size[0] = 1;
6917 b_basename->size[1] = newrobot->Base.NameInternal->size[1];
6918 cartes_emxEnsureCapacity_char_T(b_basename, localB->b_kstr_e, localB);
6919 localB->loop_ub_h = newrobot->Base.NameInternal->size[0] *
6920 newrobot->Base.NameInternal->size[1] - 1;
6921 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6922 localB->b_kstr_e++) {
6923 b_basename->data[localB->b_kstr_e] = newrobot->Base.NameInternal->
6924 data[localB->b_kstr_e];
6925 }
6926
6927 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
6928 localB->bid_m = 0.0;
6929 } else {
6930 localB->b_index = newrobot->NumBodies;
6931 localB->b_i_h = 0;
6932 exitg1 = false;
6933 while ((!exitg1) && (localB->b_i_h <= static_cast<int32_T>(localB->b_index)
6934 - 1)) {
6935 body_0 = newrobot->Bodies[localB->b_i_h];
6936 localB->b_kstr_e = b_basename->size[0] * b_basename->size[1];
6937 b_basename->size[0] = 1;
6938 b_basename->size[1] = body_0->NameInternal->size[1];
6939 cartes_emxEnsureCapacity_char_T(b_basename, localB->b_kstr_e, localB);
6940 localB->loop_ub_h = body_0->NameInternal->size[0] * body_0->
6941 NameInternal->size[1] - 1;
6942 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6943 localB->b_kstr_e++) {
6944 b_basename->data[localB->b_kstr_e] = body_0->NameInternal->data
6945 [localB->b_kstr_e];
6946 }
6947
6948 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
6949 localB->bid_m = static_cast<real_T>(localB->b_i_h) + 1.0;
6950 exitg1 = true;
6951 } else {
6952 localB->b_i_h++;
6953 }
6954 }
6955 }
6956
6957 localB->b_index = newrobot->NumBodies + 1.0;
6958 body_0 = cartesian_trajec_RigidBody_copy(body, iobj_37, iobj_38, iobj_14,
6959 localB);
6960 newrobot->Bodies[static_cast<int32_T>(localB->b_index) - 1] = body_0;
6961 body_0->Index = localB->b_index;
6962 body_0->ParentIndex = localB->bid_m;
6963 body_0->JointInternal->InTree = true;
6964 newrobot->NumBodies++;
6965 jnt = body_0->JointInternal;
6966 localB->b_kstr_e = bname->size[0] * bname->size[1];
6967 bname->size[0] = 1;
6968 bname->size[1] = jnt->Type->size[1];
6969 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr_e, localB);
6970 localB->loop_ub_h = jnt->Type->size[0] * jnt->Type->size[1] - 1;
6971 for (localB->b_kstr_e = 0; localB->b_kstr_e <= localB->loop_ub_h;
6972 localB->b_kstr_e++) {
6973 bname->data[localB->b_kstr_e] = jnt->Type->data[localB->b_kstr_e];
6974 }
6975
6976 for (localB->b_kstr_e = 0; localB->b_kstr_e < 5; localB->b_kstr_e++) {
6977 localB->b_c5[localB->b_kstr_e] = tmp[localB->b_kstr_e];
6978 }
6979
6980 localB->b_bool_c = false;
6981 if (bname->size[1] == 5) {
6982 localB->b_kstr_e = 1;
6983 do {
6984 exitg2 = 0;
6985 if (localB->b_kstr_e - 1 < 5) {
6986 localB->loop_ub_h = localB->b_kstr_e - 1;
6987 if (bname->data[localB->loop_ub_h] != localB->b_c5[localB->loop_ub_h])
6988 {
6989 exitg2 = 1;
6990 } else {
6991 localB->b_kstr_e++;
6992 }
6993 } else {
6994 localB->b_bool_c = true;
6995 exitg2 = 1;
6996 }
6997 } while (exitg2 == 0);
6998 }
6999
7000 if (!localB->b_bool_c) {
7001 newrobot->NumNonFixedBodies++;
7002 jnt = body_0->JointInternal;
7003 localB->b_kstr_e = static_cast<int32_T>(body_0->Index) - 1;
7004 newrobot->PositionDoFMap[localB->b_kstr_e] = newrobot->PositionNumber +
7005 1.0;
7006 newrobot->PositionDoFMap[localB->b_kstr_e + 8] = newrobot->PositionNumber
7007 + jnt->PositionNumber;
7008 jnt = body_0->JointInternal;
7009 localB->b_kstr_e = static_cast<int32_T>(body_0->Index) - 1;
7010 newrobot->VelocityDoFMap[localB->b_kstr_e] = newrobot->VelocityNumber +
7011 1.0;
7012 newrobot->VelocityDoFMap[localB->b_kstr_e + 8] = newrobot->VelocityNumber
7013 + jnt->VelocityNumber;
7014 } else {
7015 localB->b_kstr_e = static_cast<int32_T>(body_0->Index);
7016 newrobot->PositionDoFMap[localB->b_kstr_e - 1] = 0.0;
7017 newrobot->PositionDoFMap[localB->b_kstr_e + 7] = -1.0;
7018 localB->b_kstr_e = static_cast<int32_T>(body_0->Index);
7019 newrobot->VelocityDoFMap[localB->b_kstr_e - 1] = 0.0;
7020 newrobot->VelocityDoFMap[localB->b_kstr_e + 7] = -1.0;
7021 }
7022
7023 jnt = body_0->JointInternal;
7024 newrobot->PositionNumber += jnt->PositionNumber;
7025 jnt = body_0->JointInternal;
7026 newrobot->VelocityNumber += jnt->VelocityNumber;
7027 }
7028
7029 cartesian_trajec_emxFree_char_T(&bname);
7030 cartesian_trajec_emxFree_char_T(&b_basename);
7031 obj->RigidBodyTreeInternal = newrobot;
7032}
7033
7034static h_robotics_core_internal_Damp_T *DampedBFGSwGradientProjection_D
7035 (h_robotics_core_internal_Damp_T *obj)
7036{
7037 h_robotics_core_internal_Damp_T *b_obj;
7038 int32_T i;
7039 static const char_T tmp[22] = { 'B', 'F', 'G', 'S', 'G', 'r', 'a', 'd', 'i',
7040 'e', 'n', 't', 'P', 'r', 'o', 'j', 'e', 'c', 't', 'i', 'o', 'n' };
7041
7042 b_obj = obj;
7043 obj->MaxNumIteration = 1500.0;
7044 obj->MaxTime = 10.0;
7045 obj->GradientTolerance = 1.0E-7;
7046 obj->SolutionTolerance = 1.0E-6;
7047 obj->ArmijoRuleBeta = 0.4;
7048 obj->ArmijoRuleSigma = 1.0E-5;
7049 obj->ConstraintsOn = true;
7050 obj->RandomRestart = true;
7051 obj->StepTolerance = 1.0E-14;
7052 for (i = 0; i < 22; i++) {
7053 obj->Name[i] = tmp[i];
7054 }
7055
7056 obj->ConstraintMatrix->size[0] = 0;
7057 obj->ConstraintMatrix->size[1] = 0;
7058 obj->ConstraintBound->size[0] = 0;
7059 obj->TimeObj.StartTime = -1.0;
7060 obj->TimeObjInternal.StartTime = -1.0;
7061 return b_obj;
7062}
7063
7064static void car_inverseKinematics_setupImpl(b_inverseKinematics_cartesian_T *obj,
7065 f_robotics_manip_internal_IKE_T *iobj_0, B_MATLABSystem_cartesian_traj_T
7066 *localB)
7067{
7068 real_T n;
7069 emxArray_real_T_cartesian_tra_T *A;
7070 emxArray_real_T_cartesian_tra_T *b;
7071 real_T m;
7072 c_rigidBodyJoint_cartesian_tr_T *joint;
7073 real_T pnum;
7074 int32_T d;
7075 int32_T b_i;
7076 emxArray_real_T_cartesian_tra_T *e;
7077 int32_T j;
7078 int32_T p;
7079 emxArray_real_T_cartesian_tra_T *s;
7080 x_robotics_manip_internal_Rig_T *obj_0;
7081 w_robotics_manip_internal_Rig_T *body;
7082 int32_T c;
7083 boolean_T b_bool;
7084 emxArray_char_T_cartesian_tra_T *a;
7085 int8_T b_I[16];
7086 int32_T m_0;
7087 real_T t;
7088 int32_T b_kstr;
7089 char_T b_0[5];
7090 real_T pnum_0;
7091 int32_T loop_ub;
7092 real_T tmp;
7093 real_T w;
7094 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
7095
7096 int32_T exitg1;
7097 cartesian_trajec_emxInit_real_T(&A, 2, localB);
7098 n = obj->RigidBodyTreeInternal->PositionNumber;
7099 b_kstr = A->size[0] * A->size[1];
7100 c = static_cast<int32_T>(n);
7101 A->size[0] = c;
7102 loop_ub = static_cast<int32_T>(2.0 * n);
7103 A->size[1] = loop_ub;
7104 cartes_emxEnsureCapacity_real_T(A, b_kstr, localB);
7105 m_0 = loop_ub * c - 1;
7106 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
7107 A->data[b_kstr] = 0.0;
7108 }
7109
7110 cartesian_trajec_emxInit_real_T(&b, 1, localB);
7111 b_kstr = b->size[0];
7112 b->size[0] = loop_ub;
7113 cartes_emxEnsureCapacity_real_T(b, b_kstr, localB);
7114 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
7115 b->data[b_kstr] = 0.0;
7116 }
7117
7118 n = 1.0;
7119 m = 1.0;
7120 pnum = obj->RigidBodyTreeInternal->NumBodies;
7121 d = static_cast<int32_T>(pnum) - 1;
7122 cartesian_trajec_emxInit_real_T(&e, 2, localB);
7123 cartesian_trajec_emxInit_real_T(&s, 2, localB);
7124 cartesian_trajec_emxInit_char_T(&a, 2, localB);
7125 if (0 <= d) {
7126 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
7127 b_0[b_kstr] = tmp_0[b_kstr];
7128 }
7129 }
7130
7131 for (b_i = 0; b_i <= d; b_i++) {
7132 body = obj->RigidBodyTreeInternal->Bodies[b_i];
7133 joint = body->JointInternal;
7134 pnum = joint->PositionNumber;
7135 b_kstr = a->size[0] * a->size[1];
7136 a->size[0] = 1;
7137 a->size[1] = joint->Type->size[1];
7138 cartes_emxEnsureCapacity_char_T(a, b_kstr, localB);
7139 loop_ub = joint->Type->size[0] * joint->Type->size[1] - 1;
7140 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
7141 a->data[b_kstr] = joint->Type->data[b_kstr];
7142 }
7143
7144 b_bool = false;
7145 if (a->size[1] == 5) {
7146 b_kstr = 1;
7147 do {
7148 exitg1 = 0;
7149 if (b_kstr - 1 < 5) {
7150 loop_ub = b_kstr - 1;
7151 if (a->data[loop_ub] != b_0[loop_ub]) {
7152 exitg1 = 1;
7153 } else {
7154 b_kstr++;
7155 }
7156 } else {
7157 b_bool = true;
7158 exitg1 = 1;
7159 }
7160 } while (exitg1 == 0);
7161 }
7162
7163 if (!b_bool) {
7164 tmp = (n + pnum) - 1.0;
7165 if (n > tmp) {
7166 j = 0;
7167 } else {
7168 j = static_cast<int32_T>(n) - 1;
7169 }
7170
7171 w = m + pnum;
7172 if (m > w - 1.0) {
7173 p = 0;
7174 } else {
7175 p = static_cast<int32_T>(m) - 1;
7176 }
7177
7178 if (pnum < 0.0) {
7179 t = 0.0;
7180 pnum_0 = 0.0;
7181 } else {
7182 t = pnum;
7183 pnum_0 = pnum;
7184 }
7185
7186 m_0 = static_cast<int32_T>(pnum_0) - 1;
7187 b_kstr = s->size[0] * s->size[1];
7188 c = static_cast<int32_T>(t);
7189 s->size[0] = c;
7190 s->size[1] = c;
7191 cartes_emxEnsureCapacity_real_T(s, b_kstr, localB);
7192 loop_ub = c * c - 1;
7193 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
7194 s->data[b_kstr] = 0.0;
7195 }
7196
7197 if (c > 0) {
7198 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
7199 s->data[b_kstr + s->size[0] * b_kstr] = 1.0;
7200 }
7201 }
7202
7203 loop_ub = s->size[1];
7204 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
7205 m_0 = s->size[0];
7206 for (c = 0; c < m_0; c++) {
7207 A->data[(j + c) + A->size[0] * (p + b_kstr)] = s->data[s->size[0] *
7208 b_kstr + c];
7209 }
7210 }
7211
7212 if (n > tmp) {
7213 j = 0;
7214 } else {
7215 j = static_cast<int32_T>(n) - 1;
7216 }
7217
7218 tmp = 2.0 * pnum + m;
7219 if (w > tmp - 1.0) {
7220 p = 0;
7221 } else {
7222 p = static_cast<int32_T>(w) - 1;
7223 }
7224
7225 if (pnum < 0.0) {
7226 t = 0.0;
7227 pnum_0 = 0.0;
7228 } else {
7229 t = pnum;
7230 pnum_0 = pnum;
7231 }
7232
7233 m_0 = static_cast<int32_T>(pnum_0) - 1;
7234 b_kstr = s->size[0] * s->size[1];
7235 c = static_cast<int32_T>(t);
7236 s->size[0] = c;
7237 s->size[1] = c;
7238 cartes_emxEnsureCapacity_real_T(s, b_kstr, localB);
7239 loop_ub = c * c - 1;
7240 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
7241 s->data[b_kstr] = 0.0;
7242 }
7243
7244 if (c > 0) {
7245 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
7246 s->data[b_kstr + s->size[0] * b_kstr] = 1.0;
7247 }
7248 }
7249
7250 loop_ub = s->size[1];
7251 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
7252 m_0 = s->size[0];
7253 for (c = 0; c < m_0; c++) {
7254 A->data[(j + c) + A->size[0] * (p + b_kstr)] = -s->data[s->size[0] *
7255 b_kstr + c];
7256 }
7257 }
7258
7259 b_kstr = e->size[0] * e->size[1];
7260 e->size[0] = joint->PositionLimitsInternal->size[0];
7261 e->size[1] = 2;
7262 cartes_emxEnsureCapacity_real_T(e, b_kstr, localB);
7263 loop_ub = joint->PositionLimitsInternal->size[0] *
7264 joint->PositionLimitsInternal->size[1] - 1;
7265 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
7266 e->data[b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
7267 }
7268
7269 b->data[static_cast<int32_T>(m) - 1] = e->data[1];
7270 b_kstr = e->size[0] * e->size[1];
7271 e->size[0] = joint->PositionLimitsInternal->size[0];
7272 e->size[1] = 2;
7273 cartes_emxEnsureCapacity_real_T(e, b_kstr, localB);
7274 loop_ub = joint->PositionLimitsInternal->size[0] *
7275 joint->PositionLimitsInternal->size[1] - 1;
7276 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
7277 e->data[b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
7278 }
7279
7280 b->data[static_cast<int32_T>(m + 1.0) - 1] = -e->data[0];
7281 m = tmp;
7282 }
7283
7284 n += pnum;
7285 }
7286
7287 cartesian_trajec_emxFree_real_T(&s);
7288 b_kstr = A->size[0] * A->size[1];
7289 c = obj->Solver->ConstraintMatrix->size[0] * obj->Solver->
7290 ConstraintMatrix->size[1];
7291 obj->Solver->ConstraintMatrix->size[0] = A->size[0];
7292 obj->Solver->ConstraintMatrix->size[1] = A->size[1];
7293 cartes_emxEnsureCapacity_real_T(obj->Solver->ConstraintMatrix, c, localB);
7294 loop_ub = b_kstr - 1;
7295 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
7296 obj->Solver->ConstraintMatrix->data[b_kstr] = A->data[b_kstr];
7297 }
7298
7299 cartesian_trajec_emxFree_real_T(&A);
7300 b_kstr = obj->Solver->ConstraintBound->size[0];
7301 obj->Solver->ConstraintBound->size[0] = b->size[0];
7302 cartes_emxEnsureCapacity_real_T(obj->Solver->ConstraintBound, b_kstr, localB);
7303 loop_ub = b->size[0];
7304 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
7305 obj->Solver->ConstraintBound->data[b_kstr] = b->data[b_kstr];
7306 }
7307
7308 obj_0 = obj->RigidBodyTreeInternal;
7309 b_kstr = e->size[0] * e->size[1];
7310 e->size[0] = static_cast<int32_T>(obj_0->PositionNumber);
7311 e->size[1] = 2;
7312 cartes_emxEnsureCapacity_real_T(e, b_kstr, localB);
7313 loop_ub = (static_cast<int32_T>(obj_0->PositionNumber) << 1) - 1;
7314 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
7315 e->data[b_kstr] = 0.0;
7316 }
7317
7318 n = 1.0;
7319 m = obj_0->NumBodies;
7320 c = static_cast<int32_T>(m) - 1;
7321 if (0 <= c) {
7322 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
7323 b_0[b_kstr] = tmp_0[b_kstr];
7324 }
7325 }
7326
7327 for (b_i = 0; b_i <= c; b_i++) {
7328 body = obj_0->Bodies[b_i];
7329 b_kstr = a->size[0] * a->size[1];
7330 a->size[0] = 1;
7331 a->size[1] = body->JointInternal->Type->size[1];
7332 cartes_emxEnsureCapacity_char_T(a, b_kstr, localB);
7333 loop_ub = body->JointInternal->Type->size[0] * body->JointInternal->
7334 Type->size[1] - 1;
7335 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
7336 a->data[b_kstr] = body->JointInternal->Type->data[b_kstr];
7337 }
7338
7339 b_bool = false;
7340 if (a->size[1] == 5) {
7341 b_kstr = 1;
7342 do {
7343 exitg1 = 0;
7344 if (b_kstr - 1 < 5) {
7345 loop_ub = b_kstr - 1;
7346 if (a->data[loop_ub] != b_0[loop_ub]) {
7347 exitg1 = 1;
7348 } else {
7349 b_kstr++;
7350 }
7351 } else {
7352 b_bool = true;
7353 exitg1 = 1;
7354 }
7355 } while (exitg1 == 0);
7356 }
7357
7358 if (!b_bool) {
7359 pnum = body->JointInternal->PositionNumber;
7360 tmp = n + pnum;
7361 if (n > tmp - 1.0) {
7362 d = 0;
7363 } else {
7364 d = static_cast<int32_T>(n) - 1;
7365 }
7366
7367 joint = body->JointInternal;
7368 loop_ub = joint->PositionLimitsInternal->size[0];
7369 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
7370 e->data[d + b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
7371 }
7372
7373 loop_ub = joint->PositionLimitsInternal->size[0];
7374 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
7375 e->data[(d + b_kstr) + e->size[0]] = joint->PositionLimitsInternal->
7376 data[b_kstr + joint->PositionLimitsInternal->size[0]];
7377 }
7378
7379 n = tmp;
7380 }
7381 }
7382
7383 cartesian_trajec_emxFree_char_T(&a);
7384 b_kstr = obj->Limits->size[0] * obj->Limits->size[1];
7385 obj->Limits->size[0] = e->size[0];
7386 obj->Limits->size[1] = 2;
7387 cartes_emxEnsureCapacity_real_T(obj->Limits, b_kstr, localB);
7388 loop_ub = e->size[0] * e->size[1] - 1;
7389 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
7390 obj->Limits->data[b_kstr] = e->data[b_kstr];
7391 }
7392
7393 cartesian_trajec_emxFree_real_T(&e);
7394 obj->Solver->ExtraArgs = iobj_0;
7395 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
7396 obj->Solver->ExtraArgs->WeightMatrix[b_kstr] = 0.0;
7397 }
7398
7399 obj->Solver->ExtraArgs->Robot = obj->RigidBodyTreeInternal;
7400 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
7401 b_I[b_kstr] = 0;
7402 }
7403
7404 b_I[0] = 1;
7405 b_I[5] = 1;
7406 b_I[10] = 1;
7407 b_I[15] = 1;
7408 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
7409 obj->Solver->ExtraArgs->Tform[b_kstr] = b_I[b_kstr];
7410 }
7411
7412 obj->Solver->ExtraArgs->BodyName->size[0] = 1;
7413 obj->Solver->ExtraArgs->BodyName->size[1] = 0;
7414 b_kstr = obj->Solver->ExtraArgs->ErrTemp->size[0];
7415 obj->Solver->ExtraArgs->ErrTemp->size[0] = 6;
7416 cartes_emxEnsureCapacity_real_T(obj->Solver->ExtraArgs->ErrTemp, b_kstr,
7417 localB);
7418 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
7419 obj->Solver->ExtraArgs->ErrTemp->data[b_kstr] = 0.0;
7420 }
7421
7422 obj->Solver->ExtraArgs->CostTemp = 0.0;
7423 b_kstr = b->size[0];
7424 b->size[0] = static_cast<int32_T>(obj->RigidBodyTreeInternal->PositionNumber);
7425 cartes_emxEnsureCapacity_real_T(b, b_kstr, localB);
7426 loop_ub = static_cast<int32_T>(obj->RigidBodyTreeInternal->PositionNumber);
7427 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
7428 b->data[b_kstr] = 0.0;
7429 }
7430
7431 b_kstr = obj->Solver->ExtraArgs->GradTemp->size[0];
7432 obj->Solver->ExtraArgs->GradTemp->size[0] = b->size[0];
7433 cartes_emxEnsureCapacity_real_T(obj->Solver->ExtraArgs->GradTemp, b_kstr,
7434 localB);
7435 loop_ub = b->size[0];
7436 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
7437 obj->Solver->ExtraArgs->GradTemp->data[b_kstr] = b->data[b_kstr];
7438 }
7439
7440 cartesian_trajec_emxFree_real_T(&b);
7441}
7442
7443static void c_inverseKinematics_setPoseGoal(b_inverseKinematics_cartesian_T *obj,
7444 const real_T tform[16], const real_T weights[6],
7445 B_MATLABSystem_cartesian_traj_T *localB)
7446{
7447 real_T weightMatrix[36];
7448 f_robotics_manip_internal_IKE_T *args;
7449 int32_T b_j;
7450 static const char_T tmp[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
7451 'e', 'e' };
7452
7453 memset(&weightMatrix[0], 0, 36U * sizeof(real_T));
7454 for (b_j = 0; b_j < 6; b_j++) {
7455 weightMatrix[b_j + 6 * b_j] = weights[b_j];
7456 }
7457
7458 args = obj->Solver->ExtraArgs;
7459 for (b_j = 0; b_j < 36; b_j++) {
7460 args->WeightMatrix[b_j] = weightMatrix[b_j];
7461 }
7462
7463 b_j = args->BodyName->size[0] * args->BodyName->size[1];
7464 args->BodyName->size[0] = 1;
7465 args->BodyName->size[1] = 11;
7466 cartes_emxEnsureCapacity_char_T(args->BodyName, b_j, localB);
7467 for (b_j = 0; b_j < 11; b_j++) {
7468 args->BodyName->data[b_j] = tmp[b_j];
7469 }
7470
7471 for (b_j = 0; b_j < 16; b_j++) {
7472 args->Tform[b_j] = tform[b_j];
7473 }
7474}
7475
7476static void RigidBodyTree_validateConfigu_j(x_robotics_manip_internal_Rig_T *obj,
7477 real_T Q[6], B_MATLABSystem_cartesian_traj_T *localB)
7478{
7479 emxArray_real_T_cartesian_tra_T *limits;
7480 boolean_T ubOK[6];
7481 boolean_T lbOK[6];
7482 real_T k;
7483 w_robotics_manip_internal_Rig_T *body;
7484 real_T pnum;
7485 int32_T c;
7486 int32_T f;
7487 int32_T ii_data[6];
7488 boolean_T b_bool;
7489 emxArray_char_T_cartesian_tra_T *a;
7490 c_rigidBodyJoint_cartesian_tr_T *obj_0;
7491 int32_T idx;
7492 int32_T b_kstr;
7493 char_T b[5];
7494 int32_T loop_ub;
7495 emxArray_real_T_cartesian_tra_T *limits_0;
7496 emxArray_real_T_cartesian_tra_T *limits_1;
7497 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
7498
7499 boolean_T guard1 = false;
7500 int32_T exitg1;
7501 boolean_T exitg2;
7502 cartesian_trajec_emxInit_real_T(&limits, 2, localB);
7503 b_kstr = limits->size[0] * limits->size[1];
7504 limits->size[0] = static_cast<int32_T>(obj->PositionNumber);
7505 limits->size[1] = 2;
7506 cartes_emxEnsureCapacity_real_T(limits, b_kstr, localB);
7507 loop_ub = (static_cast<int32_T>(obj->PositionNumber) << 1) - 1;
7508 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
7509 limits->data[b_kstr] = 0.0;
7510 }
7511
7512 k = 1.0;
7513 pnum = obj->NumBodies;
7514 c = static_cast<int32_T>(pnum) - 1;
7515 cartesian_trajec_emxInit_char_T(&a, 2, localB);
7516 if (0 <= c) {
7517 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
7518 b[b_kstr] = tmp[b_kstr];
7519 }
7520 }
7521
7522 for (idx = 0; idx <= c; idx++) {
7523 body = obj->Bodies[idx];
7524 b_kstr = a->size[0] * a->size[1];
7525 a->size[0] = 1;
7526 a->size[1] = body->JointInternal->Type->size[1];
7527 cartes_emxEnsureCapacity_char_T(a, b_kstr, localB);
7528 loop_ub = body->JointInternal->Type->size[0] * body->JointInternal->
7529 Type->size[1] - 1;
7530 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
7531 a->data[b_kstr] = body->JointInternal->Type->data[b_kstr];
7532 }
7533
7534 b_bool = false;
7535 if (a->size[1] == 5) {
7536 b_kstr = 1;
7537 do {
7538 exitg1 = 0;
7539 if (b_kstr - 1 < 5) {
7540 loop_ub = b_kstr - 1;
7541 if (a->data[loop_ub] != b[loop_ub]) {
7542 exitg1 = 1;
7543 } else {
7544 b_kstr++;
7545 }
7546 } else {
7547 b_bool = true;
7548 exitg1 = 1;
7549 }
7550 } while (exitg1 == 0);
7551 }
7552
7553 if (!b_bool) {
7554 pnum = body->JointInternal->PositionNumber;
7555 pnum += k;
7556 if (k > pnum - 1.0) {
7557 f = 0;
7558 } else {
7559 f = static_cast<int32_T>(k) - 1;
7560 }
7561
7562 obj_0 = body->JointInternal;
7563 loop_ub = obj_0->PositionLimitsInternal->size[0];
7564 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
7565 limits->data[f + b_kstr] = obj_0->PositionLimitsInternal->data[b_kstr];
7566 }
7567
7568 loop_ub = obj_0->PositionLimitsInternal->size[0];
7569 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
7570 limits->data[(f + b_kstr) + limits->size[0]] =
7571 obj_0->PositionLimitsInternal->data[b_kstr +
7572 obj_0->PositionLimitsInternal->size[0]];
7573 }
7574
7575 k = pnum;
7576 }
7577 }
7578
7579 cartesian_trajec_emxFree_char_T(&a);
7580 cartesian_trajec_emxInit_real_T(&limits_0, 1, localB);
7581 loop_ub = limits->size[0];
7582 b_kstr = limits_0->size[0];
7583 limits_0->size[0] = loop_ub;
7584 cartes_emxEnsureCapacity_real_T(limits_0, b_kstr, localB);
7585 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
7586 limits_0->data[b_kstr] = limits->data[b_kstr + limits->size[0]] +
7587 4.4408920985006262E-16;
7588 }
7589
7590 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
7591 ubOK[b_kstr] = (Q[b_kstr] <= limits_0->data[b_kstr]);
7592 }
7593
7594 cartesian_trajec_emxFree_real_T(&limits_0);
7595 cartesian_trajec_emxInit_real_T(&limits_1, 1, localB);
7596 loop_ub = limits->size[0];
7597 b_kstr = limits_1->size[0];
7598 limits_1->size[0] = loop_ub;
7599 cartes_emxEnsureCapacity_real_T(limits_1, b_kstr, localB);
7600 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
7601 limits_1->data[b_kstr] = limits->data[b_kstr] - 4.4408920985006262E-16;
7602 }
7603
7604 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
7605 lbOK[b_kstr] = (Q[b_kstr] >= limits_1->data[b_kstr]);
7606 }
7607
7608 cartesian_trajec_emxFree_real_T(&limits_1);
7609 b_bool = true;
7610 idx = 0;
7611 exitg2 = false;
7612 while ((!exitg2) && (idx < 6)) {
7613 if (!ubOK[idx]) {
7614 b_bool = false;
7615 exitg2 = true;
7616 } else {
7617 idx++;
7618 }
7619 }
7620
7621 guard1 = false;
7622 if (b_bool) {
7623 b_bool = true;
7624 idx = 0;
7625 exitg2 = false;
7626 while ((!exitg2) && (idx < 6)) {
7627 if (!lbOK[idx]) {
7628 b_bool = false;
7629 exitg2 = true;
7630 } else {
7631 idx++;
7632 }
7633 }
7634
7635 if (b_bool) {
7636 } else {
7637 guard1 = true;
7638 }
7639 } else {
7640 guard1 = true;
7641 }
7642
7643 if (guard1) {
7644 idx = 0;
7645 b_kstr = 1;
7646 exitg2 = false;
7647 while ((!exitg2) && (b_kstr - 1 < 6)) {
7648 if (!ubOK[b_kstr - 1]) {
7649 idx++;
7650 ii_data[idx - 1] = b_kstr;
7651 if (idx >= 6) {
7652 exitg2 = true;
7653 } else {
7654 b_kstr++;
7655 }
7656 } else {
7657 b_kstr++;
7658 }
7659 }
7660
7661 if (1 > idx) {
7662 idx = 0;
7663 }
7664
7665 for (b_kstr = 0; b_kstr < idx; b_kstr++) {
7666 Q[ii_data[b_kstr] - 1] = limits->data[(ii_data[b_kstr] + limits->size[0])
7667 - 1];
7668 }
7669
7670 idx = 0;
7671 b_kstr = 1;
7672 exitg2 = false;
7673 while ((!exitg2) && (b_kstr - 1 < 6)) {
7674 if (!lbOK[b_kstr - 1]) {
7675 idx++;
7676 ii_data[idx - 1] = b_kstr;
7677 if (idx >= 6) {
7678 exitg2 = true;
7679 } else {
7680 b_kstr++;
7681 }
7682 } else {
7683 b_kstr++;
7684 }
7685 }
7686
7687 if (1 > idx) {
7688 idx = 0;
7689 }
7690
7691 for (b_kstr = 0; b_kstr < idx; b_kstr++) {
7692 Q[ii_data[b_kstr] - 1] = limits->data[ii_data[b_kstr] - 1];
7693 }
7694 }
7695
7696 cartesian_trajec_emxFree_real_T(&limits);
7697}
7698
7699static void ca_rigidBodyJoint_get_JointAxis(const
7700 c_rigidBodyJoint_cartesian_tr_T *obj, real_T ax[3],
7701 B_MATLABSystem_cartesian_traj_T *localB)
7702{
7703 emxArray_char_T_cartesian_tra_T *a;
7704 static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
7705
7706 static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
7707
7708 boolean_T guard1 = false;
7709 int32_T exitg1;
7710 cartesian_trajec_emxInit_char_T(&a, 2, localB);
7711 localB->b_kstr_j = a->size[0] * a->size[1];
7712 a->size[0] = 1;
7713 a->size[1] = obj->Type->size[1];
7714 cartes_emxEnsureCapacity_char_T(a, localB->b_kstr_j, localB);
7715 localB->loop_ub_o = obj->Type->size[0] * obj->Type->size[1] - 1;
7716 for (localB->b_kstr_j = 0; localB->b_kstr_j <= localB->loop_ub_o;
7717 localB->b_kstr_j++) {
7718 a->data[localB->b_kstr_j] = obj->Type->data[localB->b_kstr_j];
7719 }
7720
7721 for (localB->b_kstr_j = 0; localB->b_kstr_j < 8; localB->b_kstr_j++) {
7722 localB->b_m4[localB->b_kstr_j] = tmp[localB->b_kstr_j];
7723 }
7724
7725 localB->b_bool_k = false;
7726 if (a->size[1] == 8) {
7727 localB->b_kstr_j = 1;
7728 do {
7729 exitg1 = 0;
7730 if (localB->b_kstr_j - 1 < 8) {
7731 localB->loop_ub_o = localB->b_kstr_j - 1;
7732 if (a->data[localB->loop_ub_o] != localB->b_m4[localB->loop_ub_o]) {
7733 exitg1 = 1;
7734 } else {
7735 localB->b_kstr_j++;
7736 }
7737 } else {
7738 localB->b_bool_k = true;
7739 exitg1 = 1;
7740 }
7741 } while (exitg1 == 0);
7742 }
7743
7744 guard1 = false;
7745 if (localB->b_bool_k) {
7746 guard1 = true;
7747 } else {
7748 localB->b_kstr_j = a->size[0] * a->size[1];
7749 a->size[0] = 1;
7750 a->size[1] = obj->Type->size[1];
7751 cartes_emxEnsureCapacity_char_T(a, localB->b_kstr_j, localB);
7752 localB->loop_ub_o = obj->Type->size[0] * obj->Type->size[1] - 1;
7753 for (localB->b_kstr_j = 0; localB->b_kstr_j <= localB->loop_ub_o;
7754 localB->b_kstr_j++) {
7755 a->data[localB->b_kstr_j] = obj->Type->data[localB->b_kstr_j];
7756 }
7757
7758 for (localB->b_kstr_j = 0; localB->b_kstr_j < 9; localB->b_kstr_j++) {
7759 localB->b_i[localB->b_kstr_j] = tmp_0[localB->b_kstr_j];
7760 }
7761
7762 localB->b_bool_k = false;
7763 if (a->size[1] == 9) {
7764 localB->b_kstr_j = 1;
7765 do {
7766 exitg1 = 0;
7767 if (localB->b_kstr_j - 1 < 9) {
7768 localB->loop_ub_o = localB->b_kstr_j - 1;
7769 if (a->data[localB->loop_ub_o] != localB->b_i[localB->loop_ub_o]) {
7770 exitg1 = 1;
7771 } else {
7772 localB->b_kstr_j++;
7773 }
7774 } else {
7775 localB->b_bool_k = true;
7776 exitg1 = 1;
7777 }
7778 } while (exitg1 == 0);
7779 }
7780
7781 if (localB->b_bool_k) {
7782 guard1 = true;
7783 } else {
7784 ax[0] = (rtNaN);
7785 ax[1] = (rtNaN);
7786 ax[2] = (rtNaN);
7787 }
7788 }
7789
7790 if (guard1) {
7791 ax[0] = obj->JointAxisInternal[0];
7792 ax[1] = obj->JointAxisInternal[1];
7793 ax[2] = obj->JointAxisInternal[2];
7794 }
7795
7796 cartesian_trajec_emxFree_char_T(&a);
7797}
7798
7799static void cartesian_trajectory_planne_cat(real_T varargin_1, real_T varargin_2,
7800 real_T varargin_3, real_T varargin_4, real_T varargin_5, real_T varargin_6,
7801 real_T varargin_7, real_T varargin_8, real_T varargin_9, real_T y[9])
7802{
7803 y[0] = varargin_1;
7804 y[1] = varargin_2;
7805 y[2] = varargin_3;
7806 y[3] = varargin_4;
7807 y[4] = varargin_5;
7808 y[5] = varargin_6;
7809 y[6] = varargin_7;
7810 y[7] = varargin_8;
7811 y[8] = varargin_9;
7812}
7813
7814static void rigidBodyJoint_transformBodyToP(const
7815 c_rigidBodyJoint_cartesian_tr_T *obj, real_T T[16],
7816 B_MATLABSystem_cartesian_traj_T *localB)
7817{
7818 emxArray_char_T_cartesian_tra_T *switch_expression;
7819 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
7820
7821 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
7822
7823 int32_T exitg1;
7824 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
7825 localB->b_kstr_a = switch_expression->size[0] * switch_expression->size[1];
7826 switch_expression->size[0] = 1;
7827 switch_expression->size[1] = obj->Type->size[1];
7828 cartes_emxEnsureCapacity_char_T(switch_expression, localB->b_kstr_a, localB);
7829 localB->loop_ub_fn = obj->Type->size[0] * obj->Type->size[1] - 1;
7830 for (localB->b_kstr_a = 0; localB->b_kstr_a <= localB->loop_ub_fn;
7831 localB->b_kstr_a++) {
7832 switch_expression->data[localB->b_kstr_a] = obj->Type->data[localB->b_kstr_a];
7833 }
7834
7835 for (localB->b_kstr_a = 0; localB->b_kstr_a < 5; localB->b_kstr_a++) {
7836 localB->b_da[localB->b_kstr_a] = tmp[localB->b_kstr_a];
7837 }
7838
7839 localB->b_bool_f = false;
7840 if (switch_expression->size[1] == 5) {
7841 localB->b_kstr_a = 1;
7842 do {
7843 exitg1 = 0;
7844 if (localB->b_kstr_a - 1 < 5) {
7845 localB->loop_ub_fn = localB->b_kstr_a - 1;
7846 if (switch_expression->data[localB->loop_ub_fn] != localB->b_da
7847 [localB->loop_ub_fn]) {
7848 exitg1 = 1;
7849 } else {
7850 localB->b_kstr_a++;
7851 }
7852 } else {
7853 localB->b_bool_f = true;
7854 exitg1 = 1;
7855 }
7856 } while (exitg1 == 0);
7857 }
7858
7859 if (localB->b_bool_f) {
7860 localB->b_kstr_a = 0;
7861 } else {
7862 for (localB->b_kstr_a = 0; localB->b_kstr_a < 8; localB->b_kstr_a++) {
7863 localB->b_a[localB->b_kstr_a] = tmp_0[localB->b_kstr_a];
7864 }
7865
7866 localB->b_bool_f = false;
7867 if (switch_expression->size[1] == 8) {
7868 localB->b_kstr_a = 1;
7869 do {
7870 exitg1 = 0;
7871 if (localB->b_kstr_a - 1 < 8) {
7872 localB->loop_ub_fn = localB->b_kstr_a - 1;
7873 if (switch_expression->data[localB->loop_ub_fn] != localB->b_a
7874 [localB->loop_ub_fn]) {
7875 exitg1 = 1;
7876 } else {
7877 localB->b_kstr_a++;
7878 }
7879 } else {
7880 localB->b_bool_f = true;
7881 exitg1 = 1;
7882 }
7883 } while (exitg1 == 0);
7884 }
7885
7886 if (localB->b_bool_f) {
7887 localB->b_kstr_a = 1;
7888 } else {
7889 localB->b_kstr_a = -1;
7890 }
7891 }
7892
7893 cartesian_trajec_emxFree_char_T(&switch_expression);
7894 switch (localB->b_kstr_a) {
7895 case 0:
7896 memset(&localB->TJ_l[0], 0, sizeof(real_T) << 4U);
7897 localB->TJ_l[0] = 1.0;
7898 localB->TJ_l[5] = 1.0;
7899 localB->TJ_l[10] = 1.0;
7900 localB->TJ_l[15] = 1.0;
7901 break;
7902
7903 case 1:
7904 ca_rigidBodyJoint_get_JointAxis(obj, localB->v_pt, localB);
7905 localB->axang_idx_0 = localB->v_pt[0];
7906 localB->axang_idx_1 = localB->v_pt[1];
7907 localB->axang_idx_2 = localB->v_pt[2];
7908 localB->b_cj = 1.0 / sqrt((localB->axang_idx_0 * localB->axang_idx_0 +
7909 localB->axang_idx_1 * localB->axang_idx_1) + localB->axang_idx_2 *
7910 localB->axang_idx_2);
7911 localB->v_pt[0] = localB->axang_idx_0 * localB->b_cj;
7912 localB->v_pt[1] = localB->axang_idx_1 * localB->b_cj;
7913 localB->v_pt[2] = localB->axang_idx_2 * localB->b_cj;
7914 localB->axang_idx_0 = localB->v_pt[1] * localB->v_pt[0] * 0.0;
7915 localB->axang_idx_1 = localB->v_pt[2] * localB->v_pt[0] * 0.0;
7916 localB->axang_idx_2 = localB->v_pt[2] * localB->v_pt[1] * 0.0;
7917 cartesian_trajectory_planne_cat(localB->v_pt[0] * localB->v_pt[0] * 0.0 +
7918 1.0, localB->axang_idx_0 - localB->v_pt[2] * 0.0, localB->axang_idx_1 +
7919 localB->v_pt[1] * 0.0, localB->axang_idx_0 + localB->v_pt[2] * 0.0,
7920 localB->v_pt[1] * localB->v_pt[1] * 0.0 + 1.0, localB->axang_idx_2 -
7921 localB->v_pt[0] * 0.0, localB->axang_idx_1 - localB->v_pt[1] * 0.0,
7922 localB->axang_idx_2 + localB->v_pt[0] * 0.0, localB->v_pt[2] *
7923 localB->v_pt[2] * 0.0 + 1.0, localB->tempR_a);
7924 for (localB->b_kstr_a = 0; localB->b_kstr_a < 3; localB->b_kstr_a++) {
7925 localB->loop_ub_fn = localB->b_kstr_a + 1;
7926 localB->R_ax[localB->loop_ub_fn - 1] = localB->tempR_a[(localB->loop_ub_fn
7927 - 1) * 3];
7928 localB->loop_ub_fn = localB->b_kstr_a + 1;
7929 localB->R_ax[localB->loop_ub_fn + 2] = localB->tempR_a[(localB->loop_ub_fn
7930 - 1) * 3 + 1];
7931 localB->loop_ub_fn = localB->b_kstr_a + 1;
7932 localB->R_ax[localB->loop_ub_fn + 5] = localB->tempR_a[(localB->loop_ub_fn
7933 - 1) * 3 + 2];
7934 }
7935
7936 memset(&localB->TJ_l[0], 0, sizeof(real_T) << 4U);
7937 for (localB->b_kstr_a = 0; localB->b_kstr_a < 3; localB->b_kstr_a++) {
7938 localB->loop_ub_fn = localB->b_kstr_a << 2;
7939 localB->TJ_l[localB->loop_ub_fn] = localB->R_ax[3 * localB->b_kstr_a];
7940 localB->TJ_l[localB->loop_ub_fn + 1] = localB->R_ax[3 * localB->b_kstr_a +
7941 1];
7942 localB->TJ_l[localB->loop_ub_fn + 2] = localB->R_ax[3 * localB->b_kstr_a +
7943 2];
7944 }
7945
7946 localB->TJ_l[15] = 1.0;
7947 break;
7948
7949 default:
7950 ca_rigidBodyJoint_get_JointAxis(obj, localB->v_pt, localB);
7951 memset(&localB->tempR_a[0], 0, 9U * sizeof(real_T));
7952 localB->tempR_a[0] = 1.0;
7953 localB->tempR_a[4] = 1.0;
7954 localB->tempR_a[8] = 1.0;
7955 for (localB->b_kstr_a = 0; localB->b_kstr_a < 3; localB->b_kstr_a++) {
7956 localB->loop_ub_fn = localB->b_kstr_a << 2;
7957 localB->TJ_l[localB->loop_ub_fn] = localB->tempR_a[3 * localB->b_kstr_a];
7958 localB->TJ_l[localB->loop_ub_fn + 1] = localB->tempR_a[3 *
7959 localB->b_kstr_a + 1];
7960 localB->TJ_l[localB->loop_ub_fn + 2] = localB->tempR_a[3 *
7961 localB->b_kstr_a + 2];
7962 localB->TJ_l[localB->b_kstr_a + 12] = localB->v_pt[localB->b_kstr_a] * 0.0;
7963 }
7964
7965 localB->TJ_l[3] = 0.0;
7966 localB->TJ_l[7] = 0.0;
7967 localB->TJ_l[11] = 0.0;
7968 localB->TJ_l[15] = 1.0;
7969 break;
7970 }
7971
7972 for (localB->b_kstr_a = 0; localB->b_kstr_a < 16; localB->b_kstr_a++) {
7973 localB->a_j[localB->b_kstr_a] = obj->JointToParentTransform[localB->b_kstr_a];
7974 }
7975
7976 for (localB->b_kstr_a = 0; localB->b_kstr_a < 16; localB->b_kstr_a++) {
7977 localB->b_d[localB->b_kstr_a] = obj->ChildToJointTransform[localB->b_kstr_a];
7978 }
7979
7980 for (localB->b_kstr_a = 0; localB->b_kstr_a < 4; localB->b_kstr_a++) {
7981 for (localB->loop_ub_fn = 0; localB->loop_ub_fn < 4; localB->loop_ub_fn++) {
7982 localB->a_tmp_tmp_k = localB->loop_ub_fn << 2;
7983 localB->a_tmp_j = localB->b_kstr_a + localB->a_tmp_tmp_k;
7984 localB->a_g[localB->a_tmp_j] = 0.0;
7985 localB->a_g[localB->a_tmp_j] += localB->TJ_l[localB->a_tmp_tmp_k] *
7986 localB->a_j[localB->b_kstr_a];
7987 localB->a_g[localB->a_tmp_j] += localB->TJ_l[localB->a_tmp_tmp_k + 1] *
7988 localB->a_j[localB->b_kstr_a + 4];
7989 localB->a_g[localB->a_tmp_j] += localB->TJ_l[localB->a_tmp_tmp_k + 2] *
7990 localB->a_j[localB->b_kstr_a + 8];
7991 localB->a_g[localB->a_tmp_j] += localB->TJ_l[localB->a_tmp_tmp_k + 3] *
7992 localB->a_j[localB->b_kstr_a + 12];
7993 }
7994
7995 for (localB->loop_ub_fn = 0; localB->loop_ub_fn < 4; localB->loop_ub_fn++) {
7996 localB->a_tmp_tmp_k = localB->loop_ub_fn << 2;
7997 localB->a_tmp_j = localB->b_kstr_a + localB->a_tmp_tmp_k;
7998 T[localB->a_tmp_j] = 0.0;
7999 T[localB->a_tmp_j] += localB->b_d[localB->a_tmp_tmp_k] * localB->
8000 a_g[localB->b_kstr_a];
8001 T[localB->a_tmp_j] += localB->b_d[localB->a_tmp_tmp_k + 1] * localB->
8002 a_g[localB->b_kstr_a + 4];
8003 T[localB->a_tmp_j] += localB->b_d[localB->a_tmp_tmp_k + 2] * localB->
8004 a_g[localB->b_kstr_a + 8];
8005 T[localB->a_tmp_j] += localB->b_d[localB->a_tmp_tmp_k + 3] * localB->
8006 a_g[localB->b_kstr_a + 12];
8007 }
8008 }
8009}
8010
8011static void rigidBodyJoint_transformBodyT_n(const
8012 c_rigidBodyJoint_cartesian_tr_T *obj, const real_T q_data[], const int32_T
8013 *q_size, real_T T[16], B_MATLABSystem_cartesian_traj_T *localB)
8014{
8015 emxArray_char_T_cartesian_tra_T *switch_expression;
8016 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
8017
8018 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
8019
8020 int32_T exitg1;
8021 cartesian_trajec_emxInit_char_T(&switch_expression, 2, localB);
8022 localB->b_kstr_i = switch_expression->size[0] * switch_expression->size[1];
8023 switch_expression->size[0] = 1;
8024 switch_expression->size[1] = obj->Type->size[1];
8025 cartes_emxEnsureCapacity_char_T(switch_expression, localB->b_kstr_i, localB);
8026 localB->loop_ub_f = obj->Type->size[0] * obj->Type->size[1] - 1;
8027 for (localB->b_kstr_i = 0; localB->b_kstr_i <= localB->loop_ub_f;
8028 localB->b_kstr_i++) {
8029 switch_expression->data[localB->b_kstr_i] = obj->Type->data[localB->b_kstr_i];
8030 }
8031
8032 for (localB->b_kstr_i = 0; localB->b_kstr_i < 5; localB->b_kstr_i++) {
8033 localB->b_hp[localB->b_kstr_i] = tmp[localB->b_kstr_i];
8034 }
8035
8036 localB->b_bool_m = false;
8037 if (switch_expression->size[1] == 5) {
8038 localB->b_kstr_i = 1;
8039 do {
8040 exitg1 = 0;
8041 if (localB->b_kstr_i - 1 < 5) {
8042 localB->loop_ub_f = localB->b_kstr_i - 1;
8043 if (switch_expression->data[localB->loop_ub_f] != localB->b_hp
8044 [localB->loop_ub_f]) {
8045 exitg1 = 1;
8046 } else {
8047 localB->b_kstr_i++;
8048 }
8049 } else {
8050 localB->b_bool_m = true;
8051 exitg1 = 1;
8052 }
8053 } while (exitg1 == 0);
8054 }
8055
8056 if (localB->b_bool_m) {
8057 localB->b_kstr_i = 0;
8058 } else {
8059 for (localB->b_kstr_i = 0; localB->b_kstr_i < 8; localB->b_kstr_i++) {
8060 localB->b_h[localB->b_kstr_i] = tmp_0[localB->b_kstr_i];
8061 }
8062
8063 localB->b_bool_m = false;
8064 if (switch_expression->size[1] == 8) {
8065 localB->b_kstr_i = 1;
8066 do {
8067 exitg1 = 0;
8068 if (localB->b_kstr_i - 1 < 8) {
8069 localB->loop_ub_f = localB->b_kstr_i - 1;
8070 if (switch_expression->data[localB->loop_ub_f] != localB->b_h
8071 [localB->loop_ub_f]) {
8072 exitg1 = 1;
8073 } else {
8074 localB->b_kstr_i++;
8075 }
8076 } else {
8077 localB->b_bool_m = true;
8078 exitg1 = 1;
8079 }
8080 } while (exitg1 == 0);
8081 }
8082
8083 if (localB->b_bool_m) {
8084 localB->b_kstr_i = 1;
8085 } else {
8086 localB->b_kstr_i = -1;
8087 }
8088 }
8089
8090 cartesian_trajec_emxFree_char_T(&switch_expression);
8091 switch (localB->b_kstr_i) {
8092 case 0:
8093 memset(&localB->TJ[0], 0, sizeof(real_T) << 4U);
8094 localB->TJ[0] = 1.0;
8095 localB->TJ[5] = 1.0;
8096 localB->TJ[10] = 1.0;
8097 localB->TJ[15] = 1.0;
8098 break;
8099
8100 case 1:
8101 ca_rigidBodyJoint_get_JointAxis(obj, localB->v_pe, localB);
8102 localB->loop_ub_f = (*q_size != 0) - 1;
8103 localB->result_data[0] = localB->v_pe[0];
8104 localB->result_data[1] = localB->v_pe[1];
8105 localB->result_data[2] = localB->v_pe[2];
8106 for (localB->b_kstr_i = 0; localB->b_kstr_i <= localB->loop_ub_f;
8107 localB->b_kstr_i++) {
8108 localB->result_data[3] = q_data[0];
8109 }
8110
8111 localB->cth = 1.0 / sqrt((localB->result_data[0] * localB->result_data[0] +
8112 localB->result_data[1] * localB->result_data[1]) + localB->result_data[2] *
8113 localB->result_data[2]);
8114 localB->v_pe[0] = localB->result_data[0] * localB->cth;
8115 localB->v_pe[1] = localB->result_data[1] * localB->cth;
8116 localB->v_pe[2] = localB->result_data[2] * localB->cth;
8117 localB->cth = cos(localB->result_data[3]);
8118 localB->sth = sin(localB->result_data[3]);
8119 localB->tempR_tmp = localB->v_pe[1] * localB->v_pe[0] * (1.0 - localB->cth);
8120 localB->tempR_tmp_n = localB->v_pe[2] * localB->sth;
8121 localB->tempR_tmp_d = localB->v_pe[2] * localB->v_pe[0] * (1.0 - localB->cth);
8122 localB->tempR_tmp_na = localB->v_pe[1] * localB->sth;
8123 localB->tempR_tmp_c = localB->v_pe[2] * localB->v_pe[1] * (1.0 - localB->cth);
8124 localB->sth *= localB->v_pe[0];
8125 cartesian_trajectory_planne_cat(localB->v_pe[0] * localB->v_pe[0] * (1.0 -
8126 localB->cth) + localB->cth, localB->tempR_tmp - localB->tempR_tmp_n,
8127 localB->tempR_tmp_d + localB->tempR_tmp_na, localB->tempR_tmp +
8128 localB->tempR_tmp_n, localB->v_pe[1] * localB->v_pe[1] * (1.0 -
8129 localB->cth) + localB->cth, localB->tempR_tmp_c - localB->sth,
8130 localB->tempR_tmp_d - localB->tempR_tmp_na, localB->tempR_tmp_c +
8131 localB->sth, localB->v_pe[2] * localB->v_pe[2] * (1.0 - localB->cth) +
8132 localB->cth, localB->tempR);
8133 for (localB->b_kstr_i = 0; localB->b_kstr_i < 3; localB->b_kstr_i++) {
8134 localB->loop_ub_f = localB->b_kstr_i + 1;
8135 localB->R_e[localB->loop_ub_f - 1] = localB->tempR[(localB->loop_ub_f - 1)
8136 * 3];
8137 localB->loop_ub_f = localB->b_kstr_i + 1;
8138 localB->R_e[localB->loop_ub_f + 2] = localB->tempR[(localB->loop_ub_f - 1)
8139 * 3 + 1];
8140 localB->loop_ub_f = localB->b_kstr_i + 1;
8141 localB->R_e[localB->loop_ub_f + 5] = localB->tempR[(localB->loop_ub_f - 1)
8142 * 3 + 2];
8143 }
8144
8145 memset(&localB->TJ[0], 0, sizeof(real_T) << 4U);
8146 for (localB->b_kstr_i = 0; localB->b_kstr_i < 3; localB->b_kstr_i++) {
8147 localB->loop_ub_f = localB->b_kstr_i << 2;
8148 localB->TJ[localB->loop_ub_f] = localB->R_e[3 * localB->b_kstr_i];
8149 localB->TJ[localB->loop_ub_f + 1] = localB->R_e[3 * localB->b_kstr_i + 1];
8150 localB->TJ[localB->loop_ub_f + 2] = localB->R_e[3 * localB->b_kstr_i + 2];
8151 }
8152
8153 localB->TJ[15] = 1.0;
8154 break;
8155
8156 default:
8157 ca_rigidBodyJoint_get_JointAxis(obj, localB->v_pe, localB);
8158 memset(&localB->tempR[0], 0, 9U * sizeof(real_T));
8159 localB->tempR[0] = 1.0;
8160 localB->tempR[4] = 1.0;
8161 localB->tempR[8] = 1.0;
8162 for (localB->b_kstr_i = 0; localB->b_kstr_i < 3; localB->b_kstr_i++) {
8163 localB->loop_ub_f = localB->b_kstr_i << 2;
8164 localB->TJ[localB->loop_ub_f] = localB->tempR[3 * localB->b_kstr_i];
8165 localB->TJ[localB->loop_ub_f + 1] = localB->tempR[3 * localB->b_kstr_i + 1];
8166 localB->TJ[localB->loop_ub_f + 2] = localB->tempR[3 * localB->b_kstr_i + 2];
8167 localB->TJ[localB->b_kstr_i + 12] = localB->v_pe[localB->b_kstr_i] *
8168 q_data[0];
8169 }
8170
8171 localB->TJ[3] = 0.0;
8172 localB->TJ[7] = 0.0;
8173 localB->TJ[11] = 0.0;
8174 localB->TJ[15] = 1.0;
8175 break;
8176 }
8177
8178 for (localB->b_kstr_i = 0; localB->b_kstr_i < 16; localB->b_kstr_i++) {
8179 localB->a[localB->b_kstr_i] = obj->JointToParentTransform[localB->b_kstr_i];
8180 }
8181
8182 for (localB->b_kstr_i = 0; localB->b_kstr_i < 16; localB->b_kstr_i++) {
8183 localB->b[localB->b_kstr_i] = obj->ChildToJointTransform[localB->b_kstr_i];
8184 }
8185
8186 for (localB->b_kstr_i = 0; localB->b_kstr_i < 4; localB->b_kstr_i++) {
8187 for (localB->loop_ub_f = 0; localB->loop_ub_f < 4; localB->loop_ub_f++) {
8188 localB->a_tmp_tmp = localB->loop_ub_f << 2;
8189 localB->a_tmp = localB->b_kstr_i + localB->a_tmp_tmp;
8190 localB->a_p[localB->a_tmp] = 0.0;
8191 localB->a_p[localB->a_tmp] += localB->TJ[localB->a_tmp_tmp] * localB->
8192 a[localB->b_kstr_i];
8193 localB->a_p[localB->a_tmp] += localB->TJ[localB->a_tmp_tmp + 1] *
8194 localB->a[localB->b_kstr_i + 4];
8195 localB->a_p[localB->a_tmp] += localB->TJ[localB->a_tmp_tmp + 2] *
8196 localB->a[localB->b_kstr_i + 8];
8197 localB->a_p[localB->a_tmp] += localB->TJ[localB->a_tmp_tmp + 3] *
8198 localB->a[localB->b_kstr_i + 12];
8199 }
8200
8201 for (localB->loop_ub_f = 0; localB->loop_ub_f < 4; localB->loop_ub_f++) {
8202 localB->a_tmp_tmp = localB->loop_ub_f << 2;
8203 localB->a_tmp = localB->b_kstr_i + localB->a_tmp_tmp;
8204 T[localB->a_tmp] = 0.0;
8205 T[localB->a_tmp] += localB->b[localB->a_tmp_tmp] * localB->a_p
8206 [localB->b_kstr_i];
8207 T[localB->a_tmp] += localB->b[localB->a_tmp_tmp + 1] * localB->a_p
8208 [localB->b_kstr_i + 4];
8209 T[localB->a_tmp] += localB->b[localB->a_tmp_tmp + 2] * localB->a_p
8210 [localB->b_kstr_i + 8];
8211 T[localB->a_tmp] += localB->b[localB->a_tmp_tmp + 3] * localB->a_p
8212 [localB->b_kstr_i + 12];
8213 }
8214 }
8215}
8216
8217static void RigidBodyTree_efficientFKAndJac(x_robotics_manip_internal_Rig_T *obj,
8218 const real_T qv[6], const emxArray_char_T_cartesian_tra_T *body1Name, real_T
8219 T_data[], int32_T T_size[2], emxArray_real_T_cartesian_tra_T *Jac,
8220 B_MATLABSystem_cartesian_traj_T *localB)
8221{
8222 w_robotics_manip_internal_Rig_T *body1;
8223 w_robotics_manip_internal_Rig_T *body2;
8224 emxArray_real_T_cartesian_tra_T *kinematicPathIndices;
8225 c_rigidBodyJoint_cartesian_tr_T *joint;
8226 emxArray_char_T_cartesian_tra_T *body2Name;
8227 emxArray_real_T_cartesian_tra_T *ancestorIndices1;
8228 emxArray_real_T_cartesian_tra_T *ancestorIndices2;
8229 emxArray_real_T_cartesian_tra_T *y;
8230 emxArray_real_T_cartesian_tra_T *b;
8231 w_robotics_manip_internal_Rig_T *body;
8232 emxArray_char_T_cartesian_tra_T *bname;
8233 emxArray_real_T_cartesian_tra_T *ancestorIndices1_0;
8234 emxArray_real_T_cartesian_tra_T *ancestorIndices2_0;
8235 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
8236
8237 boolean_T exitg1;
8238 int32_T exitg2;
8239 cartesian_trajec_emxInit_char_T(&body2Name, 2, localB);
8240 localB->b_kstr = body2Name->size[0] * body2Name->size[1];
8241 body2Name->size[0] = 1;
8242 body2Name->size[1] = obj->Base.NameInternal->size[1];
8243 cartes_emxEnsureCapacity_char_T(body2Name, localB->b_kstr, localB);
8244 localB->loop_ub_a = obj->Base.NameInternal->size[0] * obj->
8245 Base.NameInternal->size[1] - 1;
8246 for (localB->b_kstr = 0; localB->b_kstr <= localB->loop_ub_a; localB->b_kstr++)
8247 {
8248 body2Name->data[localB->b_kstr] = obj->Base.NameInternal->data
8249 [localB->b_kstr];
8250 }
8251
8252 cartesian_trajec_emxInit_char_T(&bname, 2, localB);
8253 localB->bid1 = -1.0;
8254 localB->b_kstr = bname->size[0] * bname->size[1];
8255 bname->size[0] = 1;
8256 bname->size[1] = obj->Base.NameInternal->size[1];
8257 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr, localB);
8258 localB->loop_ub_a = obj->Base.NameInternal->size[0] * obj->
8259 Base.NameInternal->size[1] - 1;
8260 for (localB->b_kstr = 0; localB->b_kstr <= localB->loop_ub_a; localB->b_kstr++)
8261 {
8262 bname->data[localB->b_kstr] = obj->Base.NameInternal->data[localB->b_kstr];
8263 }
8264
8265 if (cartesian_trajectory_pla_strcmp(bname, body1Name)) {
8266 localB->bid1 = 0.0;
8267 } else {
8268 localB->qidx_idx_1 = obj->NumBodies;
8269 localB->b_i_e = 0;
8270 exitg1 = false;
8271 while ((!exitg1) && (localB->b_i_e <= static_cast<int32_T>
8272 (localB->qidx_idx_1) - 1)) {
8273 body1 = obj->Bodies[localB->b_i_e];
8274 localB->b_kstr = bname->size[0] * bname->size[1];
8275 bname->size[0] = 1;
8276 bname->size[1] = body1->NameInternal->size[1];
8277 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr, localB);
8278 localB->loop_ub_a = body1->NameInternal->size[0] * body1->
8279 NameInternal->size[1] - 1;
8280 for (localB->b_kstr = 0; localB->b_kstr <= localB->loop_ub_a;
8281 localB->b_kstr++) {
8282 bname->data[localB->b_kstr] = body1->NameInternal->data[localB->b_kstr];
8283 }
8284
8285 if (cartesian_trajectory_pla_strcmp(bname, body1Name)) {
8286 localB->bid1 = static_cast<real_T>(localB->b_i_e) + 1.0;
8287 exitg1 = true;
8288 } else {
8289 localB->b_i_e++;
8290 }
8291 }
8292 }
8293
8294 localB->bid2 = -1.0;
8295 localB->b_kstr = bname->size[0] * bname->size[1];
8296 bname->size[0] = 1;
8297 bname->size[1] = obj->Base.NameInternal->size[1];
8298 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr, localB);
8299 localB->loop_ub_a = obj->Base.NameInternal->size[0] * obj->
8300 Base.NameInternal->size[1] - 1;
8301 for (localB->b_kstr = 0; localB->b_kstr <= localB->loop_ub_a; localB->b_kstr++)
8302 {
8303 bname->data[localB->b_kstr] = obj->Base.NameInternal->data[localB->b_kstr];
8304 }
8305
8306 if (cartesian_trajectory_pla_strcmp(bname, body2Name)) {
8307 localB->bid2 = 0.0;
8308 } else {
8309 localB->qidx_idx_1 = obj->NumBodies;
8310 localB->b_i_e = 0;
8311 exitg1 = false;
8312 while ((!exitg1) && (localB->b_i_e <= static_cast<int32_T>
8313 (localB->qidx_idx_1) - 1)) {
8314 body1 = obj->Bodies[localB->b_i_e];
8315 localB->b_kstr = bname->size[0] * bname->size[1];
8316 bname->size[0] = 1;
8317 bname->size[1] = body1->NameInternal->size[1];
8318 cartes_emxEnsureCapacity_char_T(bname, localB->b_kstr, localB);
8319 localB->loop_ub_a = body1->NameInternal->size[0] * body1->
8320 NameInternal->size[1] - 1;
8321 for (localB->b_kstr = 0; localB->b_kstr <= localB->loop_ub_a;
8322 localB->b_kstr++) {
8323 bname->data[localB->b_kstr] = body1->NameInternal->data[localB->b_kstr];
8324 }
8325
8326 if (cartesian_trajectory_pla_strcmp(bname, body2Name)) {
8327 localB->bid2 = static_cast<real_T>(localB->b_i_e) + 1.0;
8328 exitg1 = true;
8329 } else {
8330 localB->b_i_e++;
8331 }
8332 }
8333 }
8334
8335 cartesian_trajec_emxFree_char_T(&bname);
8336 if (localB->bid1 == 0.0) {
8337 body1 = &obj->Base;
8338 } else {
8339 body1 = obj->Bodies[static_cast<int32_T>(localB->bid1) - 1];
8340 }
8341
8342 if (localB->bid2 == 0.0) {
8343 body2 = &obj->Base;
8344 } else {
8345 body2 = obj->Bodies[static_cast<int32_T>(localB->bid2) - 1];
8346 }
8347
8348 cartesian_trajec_emxInit_real_T(&ancestorIndices1, 2, localB);
8349 body = body1;
8350 localB->b_kstr = ancestorIndices1->size[0] * ancestorIndices1->size[1];
8351 ancestorIndices1->size[0] = 1;
8352 ancestorIndices1->size[1] = static_cast<int32_T>(obj->NumBodies + 1.0);
8353 cartes_emxEnsureCapacity_real_T(ancestorIndices1, localB->b_kstr, localB);
8354 localB->loop_ub_a = static_cast<int32_T>(obj->NumBodies + 1.0) - 1;
8355 for (localB->b_kstr = 0; localB->b_kstr <= localB->loop_ub_a; localB->b_kstr++)
8356 {
8357 ancestorIndices1->data[localB->b_kstr] = 0.0;
8358 }
8359
8360 localB->bid1 = 2.0;
8361 ancestorIndices1->data[0] = body1->Index;
8362 while (body->ParentIndex > 0.0) {
8363 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
8364 ancestorIndices1->data[static_cast<int32_T>(localB->bid1) - 1] = body->Index;
8365 localB->bid1++;
8366 }
8367
8368 if (body->Index > 0.0) {
8369 ancestorIndices1->data[static_cast<int32_T>(localB->bid1) - 1] =
8370 body->ParentIndex;
8371 localB->bid1++;
8372 }
8373
8374 cartesian_trajec_emxInit_real_T(&ancestorIndices1_0, 2, localB);
8375 localB->loop_ub_a = static_cast<int32_T>(localB->bid1 - 1.0);
8376 localB->b_kstr = ancestorIndices1_0->size[0] * ancestorIndices1_0->size[1];
8377 ancestorIndices1_0->size[0] = 1;
8378 ancestorIndices1_0->size[1] = localB->loop_ub_a;
8379 cartes_emxEnsureCapacity_real_T(ancestorIndices1_0, localB->b_kstr, localB);
8380 for (localB->b_kstr = 0; localB->b_kstr < localB->loop_ub_a; localB->b_kstr++)
8381 {
8382 ancestorIndices1_0->data[localB->b_kstr] = ancestorIndices1->data
8383 [localB->b_kstr];
8384 }
8385
8386 localB->b_kstr = ancestorIndices1->size[0] * ancestorIndices1->size[1];
8387 ancestorIndices1->size[0] = 1;
8388 ancestorIndices1->size[1] = ancestorIndices1_0->size[1];
8389 cartes_emxEnsureCapacity_real_T(ancestorIndices1, localB->b_kstr, localB);
8390 localB->loop_ub_a = ancestorIndices1_0->size[0] * ancestorIndices1_0->size[1];
8391 for (localB->b_kstr = 0; localB->b_kstr < localB->loop_ub_a; localB->b_kstr++)
8392 {
8393 ancestorIndices1->data[localB->b_kstr] = ancestorIndices1_0->data
8394 [localB->b_kstr];
8395 }
8396
8397 cartesian_trajec_emxFree_real_T(&ancestorIndices1_0);
8398 cartesian_trajec_emxInit_real_T(&ancestorIndices2, 2, localB);
8399 body = body2;
8400 localB->b_kstr = ancestorIndices2->size[0] * ancestorIndices2->size[1];
8401 ancestorIndices2->size[0] = 1;
8402 ancestorIndices2->size[1] = static_cast<int32_T>(obj->NumBodies + 1.0);
8403 cartes_emxEnsureCapacity_real_T(ancestorIndices2, localB->b_kstr, localB);
8404 localB->loop_ub_a = static_cast<int32_T>(obj->NumBodies + 1.0) - 1;
8405 for (localB->b_kstr = 0; localB->b_kstr <= localB->loop_ub_a; localB->b_kstr++)
8406 {
8407 ancestorIndices2->data[localB->b_kstr] = 0.0;
8408 }
8409
8410 localB->bid1 = 2.0;
8411 ancestorIndices2->data[0] = body2->Index;
8412 while (body->ParentIndex > 0.0) {
8413 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
8414 ancestorIndices2->data[static_cast<int32_T>(localB->bid1) - 1] = body->Index;
8415 localB->bid1++;
8416 }
8417
8418 if (body->Index > 0.0) {
8419 ancestorIndices2->data[static_cast<int32_T>(localB->bid1) - 1] =
8420 body->ParentIndex;
8421 localB->bid1++;
8422 }
8423
8424 cartesian_trajec_emxInit_real_T(&ancestorIndices2_0, 2, localB);
8425 localB->loop_ub_a = static_cast<int32_T>(localB->bid1 - 1.0);
8426 localB->b_kstr = ancestorIndices2_0->size[0] * ancestorIndices2_0->size[1];
8427 ancestorIndices2_0->size[0] = 1;
8428 ancestorIndices2_0->size[1] = localB->loop_ub_a;
8429 cartes_emxEnsureCapacity_real_T(ancestorIndices2_0, localB->b_kstr, localB);
8430 for (localB->b_kstr = 0; localB->b_kstr < localB->loop_ub_a; localB->b_kstr++)
8431 {
8432 ancestorIndices2_0->data[localB->b_kstr] = ancestorIndices2->data
8433 [localB->b_kstr];
8434 }
8435
8436 localB->b_kstr = ancestorIndices2->size[0] * ancestorIndices2->size[1];
8437 ancestorIndices2->size[0] = 1;
8438 ancestorIndices2->size[1] = ancestorIndices2_0->size[1];
8439 cartes_emxEnsureCapacity_real_T(ancestorIndices2, localB->b_kstr, localB);
8440 localB->loop_ub_a = ancestorIndices2_0->size[0] * ancestorIndices2_0->size[1];
8441 for (localB->b_kstr = 0; localB->b_kstr < localB->loop_ub_a; localB->b_kstr++)
8442 {
8443 ancestorIndices2->data[localB->b_kstr] = ancestorIndices2_0->data
8444 [localB->b_kstr];
8445 }
8446
8447 cartesian_trajec_emxFree_real_T(&ancestorIndices2_0);
8448 localB->bid1 = ancestorIndices1->size[1];
8449 localB->qidx_idx_1 = ancestorIndices2->size[1];
8450 if (localB->bid1 < localB->qidx_idx_1) {
8451 localB->qidx_idx_1 = localB->bid1;
8452 }
8453
8454 localB->bid1 = static_cast<int32_T>(localB->qidx_idx_1);
8455 localB->b_i_e = 0;
8456 exitg1 = false;
8457 while ((!exitg1) && (localB->b_i_e <= static_cast<int32_T>(localB->qidx_idx_1)
8458 - 2)) {
8459 if (ancestorIndices1->data[static_cast<int32_T>(static_cast<real_T>
8460 (ancestorIndices1->size[1]) - (static_cast<real_T>(localB->b_i_e) + 1.0))
8461 - 1] != ancestorIndices2->data[static_cast<int32_T>(static_cast<real_T>
8462 (ancestorIndices2->size[1]) - (static_cast<real_T>(localB->b_i_e) + 1.0))
8463 - 1]) {
8464 localB->bid1 = static_cast<real_T>(localB->b_i_e) + 1.0;
8465 exitg1 = true;
8466 } else {
8467 localB->b_i_e++;
8468 }
8469 }
8470
8471 localB->qidx_idx_1 = static_cast<real_T>(ancestorIndices1->size[1]) -
8472 localB->bid1;
8473 if (1.0 > localB->qidx_idx_1) {
8474 localB->b_i_e = -1;
8475 } else {
8476 localB->b_i_e = static_cast<int32_T>(localB->qidx_idx_1) - 1;
8477 }
8478
8479 localB->qidx_idx_1 = static_cast<real_T>(ancestorIndices2->size[1]) -
8480 localB->bid1;
8481 if (1.0 > localB->qidx_idx_1) {
8482 localB->j_e = 0;
8483 localB->jointSign = 1;
8484 localB->g = -1;
8485 } else {
8486 localB->j_e = static_cast<int32_T>(localB->qidx_idx_1) - 1;
8487 localB->jointSign = -1;
8488 localB->g = 0;
8489 }
8490
8491 cartesian_trajec_emxInit_real_T(&kinematicPathIndices, 2, localB);
8492 localB->b_kstr = kinematicPathIndices->size[0] * kinematicPathIndices->size[1];
8493 kinematicPathIndices->size[0] = 1;
8494 localB->loop_ub_a = div_s32_floor(localB->g - localB->j_e, localB->jointSign);
8495 kinematicPathIndices->size[1] = (localB->loop_ub_a + localB->b_i_e) + 3;
8496 cartes_emxEnsureCapacity_real_T(kinematicPathIndices, localB->b_kstr, localB);
8497 for (localB->b_kstr = 0; localB->b_kstr <= localB->b_i_e; localB->b_kstr++) {
8498 kinematicPathIndices->data[localB->b_kstr] = ancestorIndices1->data
8499 [localB->b_kstr];
8500 }
8501
8502 kinematicPathIndices->data[localB->b_i_e + 1] = ancestorIndices1->data[
8503 static_cast<int32_T>((static_cast<real_T>(ancestorIndices1->size[1]) -
8504 localB->bid1) + 1.0) - 1];
8505 cartesian_trajec_emxFree_real_T(&ancestorIndices1);
8506 for (localB->b_kstr = 0; localB->b_kstr <= localB->loop_ub_a; localB->b_kstr++)
8507 {
8508 kinematicPathIndices->data[(localB->b_kstr + localB->b_i_e) + 2] =
8509 ancestorIndices2->data[localB->jointSign * localB->b_kstr + localB->j_e];
8510 }
8511
8512 cartesian_trajec_emxFree_real_T(&ancestorIndices2);
8513 memset(&localB->T1[0], 0, sizeof(real_T) << 4U);
8514 localB->T1[0] = 1.0;
8515 localB->T1[5] = 1.0;
8516 localB->T1[10] = 1.0;
8517 localB->T1[15] = 1.0;
8518 localB->b_kstr = Jac->size[0] * Jac->size[1];
8519 Jac->size[0] = 6;
8520 Jac->size[1] = static_cast<int32_T>(obj->PositionNumber);
8521 cartes_emxEnsureCapacity_real_T(Jac, localB->b_kstr, localB);
8522 localB->loop_ub_a = 6 * static_cast<int32_T>(obj->PositionNumber) - 1;
8523 for (localB->b_kstr = 0; localB->b_kstr <= localB->loop_ub_a; localB->b_kstr++)
8524 {
8525 Jac->data[localB->b_kstr] = 0.0;
8526 }
8527
8528 localB->j_e = kinematicPathIndices->size[1] - 2;
8529 cartesian_trajec_emxInit_real_T(&y, 2, localB);
8530 cartesian_trajec_emxInit_real_T(&b, 2, localB);
8531 if (0 <= localB->j_e) {
8532 for (localB->b_kstr = 0; localB->b_kstr < 5; localB->b_kstr++) {
8533 localB->b_c2[localB->b_kstr] = tmp[localB->b_kstr];
8534 }
8535 }
8536
8537 for (localB->b_i_e = 0; localB->b_i_e <= localB->j_e; localB->b_i_e++) {
8538 if (kinematicPathIndices->data[localB->b_i_e] != 0.0) {
8539 body1 = obj->Bodies[static_cast<int32_T>(kinematicPathIndices->data
8540 [localB->b_i_e]) - 1];
8541 } else {
8542 body1 = &obj->Base;
8543 }
8544
8545 localB->b_kstr = static_cast<int32_T>((static_cast<real_T>(localB->b_i_e) +
8546 1.0) + 1.0) - 1;
8547 if (kinematicPathIndices->data[localB->b_kstr] != 0.0) {
8548 body2 = obj->Bodies[static_cast<int32_T>(kinematicPathIndices->data
8549 [localB->b_kstr]) - 1];
8550 } else {
8551 body2 = &obj->Base;
8552 }
8553
8554 localB->nextBodyIsParent = (body2->Index == body1->ParentIndex);
8555 if (localB->nextBodyIsParent) {
8556 body2 = body1;
8557 localB->jointSign = 1;
8558 } else {
8559 localB->jointSign = -1;
8560 }
8561
8562 joint = body2->JointInternal;
8563 localB->b_kstr = body2Name->size[0] * body2Name->size[1];
8564 body2Name->size[0] = 1;
8565 body2Name->size[1] = joint->Type->size[1];
8566 cartes_emxEnsureCapacity_char_T(body2Name, localB->b_kstr, localB);
8567 localB->loop_ub_a = joint->Type->size[0] * joint->Type->size[1] - 1;
8568 for (localB->b_kstr = 0; localB->b_kstr <= localB->loop_ub_a; localB->b_kstr
8569 ++) {
8570 body2Name->data[localB->b_kstr] = joint->Type->data[localB->b_kstr];
8571 }
8572
8573 localB->b_bool_p = false;
8574 if (body2Name->size[1] == 5) {
8575 localB->b_kstr = 1;
8576 do {
8577 exitg2 = 0;
8578 if (localB->b_kstr - 1 < 5) {
8579 localB->g = localB->b_kstr - 1;
8580 if (body2Name->data[localB->g] != localB->b_c2[localB->g]) {
8581 exitg2 = 1;
8582 } else {
8583 localB->b_kstr++;
8584 }
8585 } else {
8586 localB->b_bool_p = true;
8587 exitg2 = 1;
8588 }
8589 } while (exitg2 == 0);
8590 }
8591
8592 if (localB->b_bool_p) {
8593 rigidBodyJoint_transformBodyToP(joint, localB->Tc2p, localB);
8594 } else {
8595 localB->b_kstr = static_cast<int32_T>(body2->Index);
8596 localB->bid1 = obj->PositionDoFMap[localB->b_kstr - 1];
8597 localB->qidx_idx_1 = obj->PositionDoFMap[localB->b_kstr + 7];
8598 if (localB->bid1 > localB->qidx_idx_1) {
8599 localB->g = 0;
8600 localB->b_kstr = -1;
8601 } else {
8602 localB->g = static_cast<int32_T>(localB->bid1) - 1;
8603 localB->b_kstr = static_cast<int32_T>(localB->qidx_idx_1) - 1;
8604 }
8605
8606 localB->loop_ub_a = localB->b_kstr - localB->g;
8607 localB->qv_size = localB->loop_ub_a + 1;
8608 for (localB->b_kstr = 0; localB->b_kstr <= localB->loop_ub_a;
8609 localB->b_kstr++) {
8610 localB->qv_data[localB->b_kstr] = qv[localB->g + localB->b_kstr];
8611 }
8612
8613 rigidBodyJoint_transformBodyT_n(joint, localB->qv_data, &localB->qv_size,
8614 localB->Tc2p, localB);
8615 localB->b_kstr = static_cast<int32_T>(body2->Index);
8616 localB->bid1 = obj->VelocityDoFMap[localB->b_kstr - 1];
8617 localB->qidx_idx_1 = obj->VelocityDoFMap[localB->b_kstr + 7];
8618 if (localB->nextBodyIsParent) {
8619 for (localB->b_kstr = 0; localB->b_kstr < 16; localB->b_kstr++) {
8620 localB->Tj[localB->b_kstr] = joint->ChildToJointTransform
8621 [localB->b_kstr];
8622 }
8623 } else {
8624 for (localB->b_kstr = 0; localB->b_kstr < 16; localB->b_kstr++) {
8625 localB->T1j[localB->b_kstr] = joint->JointToParentTransform
8626 [localB->b_kstr];
8627 }
8628
8629 for (localB->b_kstr = 0; localB->b_kstr < 3; localB->b_kstr++) {
8630 localB->R[3 * localB->b_kstr] = localB->T1j[localB->b_kstr];
8631 localB->R[3 * localB->b_kstr + 1] = localB->T1j[localB->b_kstr + 4];
8632 localB->R[3 * localB->b_kstr + 2] = localB->T1j[localB->b_kstr + 8];
8633 }
8634
8635 for (localB->b_kstr = 0; localB->b_kstr < 9; localB->b_kstr++) {
8636 localB->R_a[localB->b_kstr] = -localB->R[localB->b_kstr];
8637 }
8638
8639 for (localB->b_kstr = 0; localB->b_kstr < 3; localB->b_kstr++) {
8640 localB->g = localB->b_kstr << 2;
8641 localB->Tj[localB->g] = localB->R[3 * localB->b_kstr];
8642 localB->Tj[localB->g + 1] = localB->R[3 * localB->b_kstr + 1];
8643 localB->Tj[localB->g + 2] = localB->R[3 * localB->b_kstr + 2];
8644 localB->Tj[localB->b_kstr + 12] = localB->R_a[localB->b_kstr + 6] *
8645 localB->T1j[14] + (localB->R_a[localB->b_kstr + 3] * localB->T1j[13]
8646 + localB->R_a[localB->b_kstr] * localB->T1j[12]);
8647 }
8648
8649 localB->Tj[3] = 0.0;
8650 localB->Tj[7] = 0.0;
8651 localB->Tj[11] = 0.0;
8652 localB->Tj[15] = 1.0;
8653 }
8654
8655 for (localB->b_kstr = 0; localB->b_kstr < 4; localB->b_kstr++) {
8656 for (localB->g = 0; localB->g < 4; localB->g++) {
8657 localB->loop_ub_a = localB->g << 2;
8658 localB->n_b = localB->b_kstr + localB->loop_ub_a;
8659 localB->T1j[localB->n_b] = 0.0;
8660 localB->T1j[localB->n_b] += localB->T1[localB->loop_ub_a] * localB->
8661 Tj[localB->b_kstr];
8662 localB->T1j[localB->n_b] += localB->T1[localB->loop_ub_a + 1] *
8663 localB->Tj[localB->b_kstr + 4];
8664 localB->T1j[localB->n_b] += localB->T1[localB->loop_ub_a + 2] *
8665 localB->Tj[localB->b_kstr + 8];
8666 localB->T1j[localB->n_b] += localB->T1[localB->loop_ub_a + 3] *
8667 localB->Tj[localB->b_kstr + 12];
8668 }
8669 }
8670
8671 for (localB->b_kstr = 0; localB->b_kstr < 3; localB->b_kstr++) {
8672 localB->R[3 * localB->b_kstr] = localB->T1j[localB->b_kstr];
8673 localB->R[3 * localB->b_kstr + 1] = localB->T1j[localB->b_kstr + 4];
8674 localB->R[3 * localB->b_kstr + 2] = localB->T1j[localB->b_kstr + 8];
8675 }
8676
8677 for (localB->b_kstr = 0; localB->b_kstr < 9; localB->b_kstr++) {
8678 localB->R_a[localB->b_kstr] = -localB->R[localB->b_kstr];
8679 }
8680
8681 for (localB->b_kstr = 0; localB->b_kstr < 3; localB->b_kstr++) {
8682 localB->g = localB->b_kstr << 2;
8683 localB->Tj[localB->g] = localB->R[3 * localB->b_kstr];
8684 localB->Tj[localB->g + 1] = localB->R[3 * localB->b_kstr + 1];
8685 localB->Tj[localB->g + 2] = localB->R[3 * localB->b_kstr + 2];
8686 localB->Tj[localB->b_kstr + 12] = localB->R_a[localB->b_kstr + 6] *
8687 localB->T1j[14] + (localB->R_a[localB->b_kstr + 3] * localB->T1j[13] +
8688 localB->R_a[localB->b_kstr] * localB->T1j[12]);
8689 }
8690
8691 localB->Tj[3] = 0.0;
8692 localB->Tj[7] = 0.0;
8693 localB->Tj[11] = 0.0;
8694 localB->Tj[15] = 1.0;
8695 localB->R[0] = 0.0;
8696 localB->R[3] = -localB->Tj[14];
8697 localB->R[6] = localB->Tj[13];
8698 localB->R[1] = localB->Tj[14];
8699 localB->R[4] = 0.0;
8700 localB->R[7] = -localB->Tj[12];
8701 localB->R[2] = -localB->Tj[13];
8702 localB->R[5] = localB->Tj[12];
8703 localB->R[8] = 0.0;
8704 for (localB->b_kstr = 0; localB->b_kstr < 3; localB->b_kstr++) {
8705 for (localB->g = 0; localB->g < 3; localB->g++) {
8706 localB->loop_ub_a = localB->b_kstr + 3 * localB->g;
8707 localB->R_a[localB->loop_ub_a] = 0.0;
8708 localB->n_b = localB->g << 2;
8709 localB->R_a[localB->loop_ub_a] += localB->Tj[localB->n_b] * localB->
8710 R[localB->b_kstr];
8711 localB->R_a[localB->loop_ub_a] += localB->Tj[localB->n_b + 1] *
8712 localB->R[localB->b_kstr + 3];
8713 localB->R_a[localB->loop_ub_a] += localB->Tj[localB->n_b + 2] *
8714 localB->R[localB->b_kstr + 6];
8715 localB->X[localB->g + 6 * localB->b_kstr] = localB->Tj[(localB->b_kstr
8716 << 2) + localB->g];
8717 localB->X[localB->g + 6 * (localB->b_kstr + 3)] = 0.0;
8718 }
8719 }
8720
8721 for (localB->b_kstr = 0; localB->b_kstr < 3; localB->b_kstr++) {
8722 localB->X[6 * localB->b_kstr + 3] = localB->R_a[3 * localB->b_kstr];
8723 localB->g = localB->b_kstr << 2;
8724 localB->loop_ub_a = 6 * (localB->b_kstr + 3);
8725 localB->X[localB->loop_ub_a + 3] = localB->Tj[localB->g];
8726 localB->X[6 * localB->b_kstr + 4] = localB->R_a[3 * localB->b_kstr + 1];
8727 localB->X[localB->loop_ub_a + 4] = localB->Tj[localB->g + 1];
8728 localB->X[6 * localB->b_kstr + 5] = localB->R_a[3 * localB->b_kstr + 2];
8729 localB->X[localB->loop_ub_a + 5] = localB->Tj[localB->g + 2];
8730 }
8731
8732 localB->b_kstr = b->size[0] * b->size[1];
8733 b->size[0] = 6;
8734 b->size[1] = joint->MotionSubspace->size[1];
8735 cartes_emxEnsureCapacity_real_T(b, localB->b_kstr, localB);
8736 localB->loop_ub_a = joint->MotionSubspace->size[0] * joint->
8737 MotionSubspace->size[1] - 1;
8738 for (localB->b_kstr = 0; localB->b_kstr <= localB->loop_ub_a;
8739 localB->b_kstr++) {
8740 b->data[localB->b_kstr] = joint->MotionSubspace->data[localB->b_kstr];
8741 }
8742
8743 localB->n_b = b->size[1] - 1;
8744 localB->b_kstr = y->size[0] * y->size[1];
8745 y->size[0] = 6;
8746 y->size[1] = b->size[1];
8747 cartes_emxEnsureCapacity_real_T(y, localB->b_kstr, localB);
8748 for (localB->b_kstr = 0; localB->b_kstr <= localB->n_b; localB->b_kstr++)
8749 {
8750 localB->coffset_tmp = localB->b_kstr * 6 - 1;
8751 for (localB->g = 0; localB->g < 6; localB->g++) {
8752 localB->bid2 = 0.0;
8753 for (localB->loop_ub_a = 0; localB->loop_ub_a < 6; localB->loop_ub_a++)
8754 {
8755 localB->bid2 += localB->X[localB->loop_ub_a * 6 + localB->g] *
8756 b->data[(localB->coffset_tmp + localB->loop_ub_a) + 1];
8757 }
8758
8759 y->data[(localB->coffset_tmp + localB->g) + 1] = localB->bid2;
8760 }
8761 }
8762
8763 if (localB->bid1 > localB->qidx_idx_1) {
8764 localB->n_b = 0;
8765 } else {
8766 localB->n_b = static_cast<int32_T>(localB->bid1) - 1;
8767 }
8768
8769 localB->loop_ub_a = y->size[1];
8770 for (localB->b_kstr = 0; localB->b_kstr < localB->loop_ub_a;
8771 localB->b_kstr++) {
8772 for (localB->g = 0; localB->g < 6; localB->g++) {
8773 Jac->data[localB->g + 6 * (localB->n_b + localB->b_kstr)] = y->data[6 *
8774 localB->b_kstr + localB->g] * static_cast<real_T>(localB->jointSign);
8775 }
8776 }
8777 }
8778
8779 if (localB->nextBodyIsParent) {
8780 for (localB->b_kstr = 0; localB->b_kstr < 4; localB->b_kstr++) {
8781 for (localB->g = 0; localB->g < 4; localB->g++) {
8782 localB->loop_ub_a = localB->g << 2;
8783 localB->jointSign = localB->b_kstr + localB->loop_ub_a;
8784 localB->T1j[localB->jointSign] = 0.0;
8785 localB->T1j[localB->jointSign] += localB->T1[localB->loop_ub_a] *
8786 localB->Tc2p[localB->b_kstr];
8787 localB->T1j[localB->jointSign] += localB->T1[localB->loop_ub_a + 1] *
8788 localB->Tc2p[localB->b_kstr + 4];
8789 localB->T1j[localB->jointSign] += localB->T1[localB->loop_ub_a + 2] *
8790 localB->Tc2p[localB->b_kstr + 8];
8791 localB->T1j[localB->jointSign] += localB->T1[localB->loop_ub_a + 3] *
8792 localB->Tc2p[localB->b_kstr + 12];
8793 }
8794 }
8795
8796 memcpy(&localB->T1[0], &localB->T1j[0], sizeof(real_T) << 4U);
8797 } else {
8798 for (localB->b_kstr = 0; localB->b_kstr < 3; localB->b_kstr++) {
8799 localB->R[3 * localB->b_kstr] = localB->Tc2p[localB->b_kstr];
8800 localB->R[3 * localB->b_kstr + 1] = localB->Tc2p[localB->b_kstr + 4];
8801 localB->R[3 * localB->b_kstr + 2] = localB->Tc2p[localB->b_kstr + 8];
8802 }
8803
8804 for (localB->b_kstr = 0; localB->b_kstr < 9; localB->b_kstr++) {
8805 localB->R_a[localB->b_kstr] = -localB->R[localB->b_kstr];
8806 }
8807
8808 for (localB->b_kstr = 0; localB->b_kstr < 3; localB->b_kstr++) {
8809 localB->loop_ub_a = localB->b_kstr << 2;
8810 localB->T1j[localB->loop_ub_a] = localB->R[3 * localB->b_kstr];
8811 localB->T1j[localB->loop_ub_a + 1] = localB->R[3 * localB->b_kstr + 1];
8812 localB->T1j[localB->loop_ub_a + 2] = localB->R[3 * localB->b_kstr + 2];
8813 localB->T1j[localB->b_kstr + 12] = localB->R_a[localB->b_kstr + 6] *
8814 localB->Tc2p[14] + (localB->R_a[localB->b_kstr + 3] * localB->Tc2p[13]
8815 + localB->R_a[localB->b_kstr] * localB->Tc2p[12]);
8816 }
8817
8818 localB->T1j[3] = 0.0;
8819 localB->T1j[7] = 0.0;
8820 localB->T1j[11] = 0.0;
8821 localB->T1j[15] = 1.0;
8822 for (localB->b_kstr = 0; localB->b_kstr < 4; localB->b_kstr++) {
8823 for (localB->g = 0; localB->g < 4; localB->g++) {
8824 localB->jointSign = localB->g << 2;
8825 localB->loop_ub_a = localB->b_kstr + localB->jointSign;
8826 localB->Tc2p[localB->loop_ub_a] = 0.0;
8827 localB->Tc2p[localB->loop_ub_a] += localB->T1[localB->jointSign] *
8828 localB->T1j[localB->b_kstr];
8829 localB->Tc2p[localB->loop_ub_a] += localB->T1[localB->jointSign + 1] *
8830 localB->T1j[localB->b_kstr + 4];
8831 localB->Tc2p[localB->loop_ub_a] += localB->T1[localB->jointSign + 2] *
8832 localB->T1j[localB->b_kstr + 8];
8833 localB->Tc2p[localB->loop_ub_a] += localB->T1[localB->jointSign + 3] *
8834 localB->T1j[localB->b_kstr + 12];
8835 }
8836 }
8837
8838 memcpy(&localB->T1[0], &localB->Tc2p[0], sizeof(real_T) << 4U);
8839 }
8840 }
8841
8842 cartesian_trajec_emxFree_real_T(&b);
8843 cartesian_trajec_emxFree_char_T(&body2Name);
8844 cartesian_trajec_emxFree_real_T(&kinematicPathIndices);
8845 for (localB->b_kstr = 0; localB->b_kstr < 3; localB->b_kstr++) {
8846 localB->b_i_e = localB->b_kstr << 2;
8847 localB->X[6 * localB->b_kstr] = localB->T1[localB->b_i_e];
8848 localB->g = 6 * (localB->b_kstr + 3);
8849 localB->X[localB->g] = 0.0;
8850 localB->X[6 * localB->b_kstr + 3] = 0.0;
8851 localB->X[localB->g + 3] = localB->T1[localB->b_i_e];
8852 localB->bid1 = localB->T1[localB->b_i_e + 1];
8853 localB->X[6 * localB->b_kstr + 1] = localB->bid1;
8854 localB->X[localB->g + 1] = 0.0;
8855 localB->X[6 * localB->b_kstr + 4] = 0.0;
8856 localB->X[localB->g + 4] = localB->bid1;
8857 localB->bid1 = localB->T1[localB->b_i_e + 2];
8858 localB->X[6 * localB->b_kstr + 2] = localB->bid1;
8859 localB->X[localB->g + 2] = 0.0;
8860 localB->X[6 * localB->b_kstr + 5] = 0.0;
8861 localB->X[localB->g + 5] = localB->bid1;
8862 }
8863
8864 localB->n_b = Jac->size[1];
8865 localB->b_kstr = y->size[0] * y->size[1];
8866 y->size[0] = 6;
8867 y->size[1] = Jac->size[1];
8868 cartes_emxEnsureCapacity_real_T(y, localB->b_kstr, localB);
8869 localB->loop_ub_a = Jac->size[0] * Jac->size[1] - 1;
8870 for (localB->b_kstr = 0; localB->b_kstr <= localB->loop_ub_a; localB->b_kstr++)
8871 {
8872 y->data[localB->b_kstr] = Jac->data[localB->b_kstr];
8873 }
8874
8875 localB->b_kstr = Jac->size[0] * Jac->size[1];
8876 Jac->size[0] = 6;
8877 Jac->size[1] = localB->n_b;
8878 cartes_emxEnsureCapacity_real_T(Jac, localB->b_kstr, localB);
8879 for (localB->b_kstr = 0; localB->b_kstr < localB->n_b; localB->b_kstr++) {
8880 localB->coffset_tmp = localB->b_kstr * 6 - 1;
8881 for (localB->b_i_e = 0; localB->b_i_e < 6; localB->b_i_e++) {
8882 localB->bid2 = 0.0;
8883 for (localB->loop_ub_a = 0; localB->loop_ub_a < 6; localB->loop_ub_a++) {
8884 localB->bid2 += localB->X[localB->loop_ub_a * 6 + localB->b_i_e] *
8885 y->data[(localB->coffset_tmp + localB->loop_ub_a) + 1];
8886 }
8887
8888 Jac->data[(localB->coffset_tmp + localB->b_i_e) + 1] = localB->bid2;
8889 }
8890 }
8891
8892 cartesian_trajec_emxFree_real_T(&y);
8893 T_size[0] = 4;
8894 T_size[1] = 4;
8895 memcpy(&T_data[0], &localB->T1[0], sizeof(real_T) << 4U);
8896}
8897
8898real_T rt_hypotd_snf(real_T u0, real_T u1)
8899{
8900 real_T y;
8901 real_T a;
8902 a = fabs(u0);
8903 y = fabs(u1);
8904 if (a < y) {
8905 a /= y;
8906 y *= sqrt(a * a + 1.0);
8907 } else if (a > y) {
8908 y /= a;
8909 y = sqrt(y * y + 1.0) * a;
8910 } else {
8911 if (!rtIsNaN(y)) {
8912 y = a * 1.4142135623730951;
8913 }
8914 }
8915
8916 return y;
8917}
8918
8919static creal_T cartesian_trajectory_plann_sqrt(const creal_T x)
8920{
8921 creal_T b_x;
8922 real_T absxr;
8923 real_T absxi;
8924 if (x.im == 0.0) {
8925 if (x.re < 0.0) {
8926 absxr = 0.0;
8927 absxi = sqrt(-x.re);
8928 } else {
8929 absxr = sqrt(x.re);
8930 absxi = 0.0;
8931 }
8932 } else if (x.re == 0.0) {
8933 if (x.im < 0.0) {
8934 absxr = sqrt(-x.im / 2.0);
8935 absxi = -absxr;
8936 } else {
8937 absxr = sqrt(x.im / 2.0);
8938 absxi = absxr;
8939 }
8940 } else if (rtIsNaN(x.re)) {
8941 absxr = x.re;
8942 absxi = x.re;
8943 } else if (rtIsNaN(x.im)) {
8944 absxr = x.im;
8945 absxi = x.im;
8946 } else if (rtIsInf(x.im)) {
8947 absxr = fabs(x.im);
8948 absxi = x.im;
8949 } else if (rtIsInf(x.re)) {
8950 if (x.re < 0.0) {
8951 absxr = 0.0;
8952 absxi = x.im * -x.re;
8953 } else {
8954 absxr = x.re;
8955 absxi = 0.0;
8956 }
8957 } else {
8958 absxr = fabs(x.re);
8959 absxi = fabs(x.im);
8960 if ((absxr > 4.4942328371557893E+307) || (absxi > 4.4942328371557893E+307))
8961 {
8962 absxr *= 0.5;
8963 absxi *= 0.5;
8964 absxi = rt_hypotd_snf(absxr, absxi);
8965 if (absxi > absxr) {
8966 absxr = sqrt(absxr / absxi + 1.0) * sqrt(absxi);
8967 } else {
8968 absxr = sqrt(absxi) * 1.4142135623730951;
8969 }
8970 } else {
8971 absxr = sqrt((rt_hypotd_snf(absxr, absxi) + absxr) * 0.5);
8972 }
8973
8974 if (x.re > 0.0) {
8975 absxi = x.im / absxr * 0.5;
8976 } else {
8977 if (x.im < 0.0) {
8978 absxi = -absxr;
8979 } else {
8980 absxi = absxr;
8981 }
8982
8983 absxr = x.im / absxi * 0.5;
8984 }
8985 }
8986
8987 b_x.re = absxr;
8988 b_x.im = absxi;
8989 return b_x;
8990}
8991
8992real_T rt_atan2d_snf(real_T u0, real_T u1)
8993{
8994 real_T y;
8995 int32_T u0_0;
8996 int32_T u1_0;
8997 if (rtIsNaN(u0) || rtIsNaN(u1)) {
8998 y = (rtNaN);
8999 } else if (rtIsInf(u0) && rtIsInf(u1)) {
9000 if (u0 > 0.0) {
9001 u0_0 = 1;
9002 } else {
9003 u0_0 = -1;
9004 }
9005
9006 if (u1 > 0.0) {
9007 u1_0 = 1;
9008 } else {
9009 u1_0 = -1;
9010 }
9011
9012 y = atan2(static_cast<real_T>(u0_0), static_cast<real_T>(u1_0));
9013 } else if (u1 == 0.0) {
9014 if (u0 > 0.0) {
9015 y = RT_PI / 2.0;
9016 } else if (u0 < 0.0) {
9017 y = -(RT_PI / 2.0);
9018 } else {
9019 y = 0.0;
9020 }
9021 } else {
9022 y = atan2(u0, u1);
9023 }
9024
9025 return y;
9026}
9027
9028static real_T cartesian_trajectory_plan_xnrm2(int32_T n, const real_T x[9],
9029 int32_T ix0)
9030{
9031 real_T y;
9032 real_T scale;
9033 int32_T kend;
9034 real_T absxk;
9035 real_T t;
9036 int32_T k;
9037 y = 0.0;
9038 scale = 3.3121686421112381E-170;
9039 kend = ix0 + n;
9040 for (k = ix0; k < kend; k++) {
9041 absxk = fabs(x[k - 1]);
9042 if (absxk > scale) {
9043 t = scale / absxk;
9044 y = y * t * t + 1.0;
9045 scale = absxk;
9046 } else {
9047 t = absxk / scale;
9048 y += t * t;
9049 }
9050 }
9051
9052 return scale * sqrt(y);
9053}
9054
9055static real_T cartesian_trajectory_pl_xnrm2_a(const real_T x[3], int32_T ix0)
9056{
9057 real_T y;
9058 real_T scale;
9059 real_T absxk;
9060 real_T t;
9061 int32_T k;
9062 y = 0.0;
9063 scale = 3.3121686421112381E-170;
9064 for (k = ix0; k <= ix0 + 1; k++) {
9065 absxk = fabs(x[k - 1]);
9066 if (absxk > scale) {
9067 t = scale / absxk;
9068 y = y * t * t + 1.0;
9069 scale = absxk;
9070 } else {
9071 t = absxk / scale;
9072 y += t * t;
9073 }
9074 }
9075
9076 return scale * sqrt(y);
9077}
9078
9079static real_T cartesian_trajectory_plan_xdotc(int32_T n, const real_T x[9],
9080 int32_T ix0, const real_T y[9], int32_T iy0)
9081{
9082 real_T d;
9083 int32_T ix;
9084 int32_T iy;
9085 int32_T k;
9086 d = 0.0;
9087 ix = ix0 - 1;
9088 iy = iy0 - 1;
9089 for (k = 0; k < n; k++) {
9090 d += x[ix] * y[iy];
9091 ix++;
9092 iy++;
9093 }
9094
9095 return d;
9096}
9097
9098static void cartesian_trajectory_plan_xaxpy(int32_T n, real_T a, int32_T ix0,
9099 const real_T y[9], int32_T iy0, real_T b_y[9])
9100{
9101 int32_T ix;
9102 int32_T iy;
9103 int32_T k;
9104 memcpy(&b_y[0], &y[0], 9U * sizeof(real_T));
9105 if (!(a == 0.0)) {
9106 ix = ix0;
9107 iy = iy0 - 1;
9108 for (k = 0; k < n; k++) {
9109 b_y[iy] += b_y[ix - 1] * a;
9110 ix++;
9111 iy++;
9112 }
9113 }
9114}
9115
9116static void cartesian_trajectory__xaxpy_cxo(int32_T n, real_T a, const real_T x
9117 [9], int32_T ix0, real_T y[3], int32_T iy0)
9118{
9119 int32_T ix;
9120 int32_T iy;
9121 int32_T k;
9122 if (!(a == 0.0)) {
9123 ix = ix0;
9124 iy = iy0 - 1;
9125 for (k = 0; k < n; k++) {
9126 y[iy] += x[ix - 1] * a;
9127 ix++;
9128 iy++;
9129 }
9130 }
9131}
9132
9133static void cartesian_trajectory_p_xaxpy_cx(int32_T n, real_T a, const real_T x
9134 [3], int32_T ix0, const real_T y[9], int32_T iy0, real_T b_y[9])
9135{
9136 int32_T ix;
9137 int32_T iy;
9138 int32_T k;
9139 memcpy(&b_y[0], &y[0], 9U * sizeof(real_T));
9140 if (!(a == 0.0)) {
9141 ix = ix0;
9142 iy = iy0 - 1;
9143 for (k = 0; k < n; k++) {
9144 b_y[iy] += x[ix - 1] * a;
9145 ix++;
9146 iy++;
9147 }
9148 }
9149}
9150
9151static void cartesian_trajectory_plan_xrotg(real_T a, real_T b, real_T *b_a,
9152 real_T *b_b, real_T *c, real_T *s, B_MATLABSystem_cartesian_traj_T *localB)
9153{
9154 localB->roe = b;
9155 localB->absa = fabs(a);
9156 localB->absb = fabs(b);
9157 if (localB->absa > localB->absb) {
9158 localB->roe = a;
9159 }
9160
9161 localB->scale = localB->absa + localB->absb;
9162 if (localB->scale == 0.0) {
9163 *s = 0.0;
9164 *c = 1.0;
9165 *b_a = 0.0;
9166 *b_b = 0.0;
9167 } else {
9168 localB->ads = localB->absa / localB->scale;
9169 localB->bds = localB->absb / localB->scale;
9170 *b_a = sqrt(localB->ads * localB->ads + localB->bds * localB->bds) *
9171 localB->scale;
9172 if (localB->roe < 0.0) {
9173 *b_a = -*b_a;
9174 }
9175
9176 *c = a / *b_a;
9177 *s = b / *b_a;
9178 if (localB->absa > localB->absb) {
9179 *b_b = *s;
9180 } else if (*c != 0.0) {
9181 *b_b = 1.0 / *c;
9182 } else {
9183 *b_b = 1.0;
9184 }
9185 }
9186}
9187
9188static void cartesian_trajectory_plann_xrot(const real_T x[9], int32_T ix0,
9189 int32_T iy0, real_T c, real_T s, real_T b_x[9])
9190{
9191 int32_T ix;
9192 int32_T iy;
9193 real_T temp;
9194 memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));
9195 ix = ix0 - 1;
9196 iy = iy0 - 1;
9197 temp = c * b_x[ix] + s * b_x[iy];
9198 b_x[iy] = c * b_x[iy] - s * b_x[ix];
9199 b_x[ix] = temp;
9200 iy++;
9201 ix++;
9202 temp = c * b_x[ix] + s * b_x[iy];
9203 b_x[iy] = c * b_x[iy] - s * b_x[ix];
9204 b_x[ix] = temp;
9205 iy++;
9206 ix++;
9207 temp = c * b_x[ix] + s * b_x[iy];
9208 b_x[iy] = c * b_x[iy] - s * b_x[ix];
9209 b_x[ix] = temp;
9210}
9211
9212static void cartesian_trajectory_plan_xswap(const real_T x[9], int32_T ix0,
9213 int32_T iy0, real_T b_x[9])
9214{
9215 int32_T ix;
9216 int32_T iy;
9217 real_T temp;
9218 memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));
9219 ix = ix0 - 1;
9220 iy = iy0 - 1;
9221 temp = b_x[ix];
9222 b_x[ix] = b_x[iy];
9223 b_x[iy] = temp;
9224 ix++;
9225 iy++;
9226 temp = b_x[ix];
9227 b_x[ix] = b_x[iy];
9228 b_x[iy] = temp;
9229 ix++;
9230 iy++;
9231 temp = b_x[ix];
9232 b_x[ix] = b_x[iy];
9233 b_x[iy] = temp;
9234}
9235
9236static void cartesian_trajectory_planne_svd(const real_T A[9], real_T U[9],
9237 real_T s[3], real_T V[9], B_MATLABSystem_cartesian_traj_T *localB)
9238{
9239 int32_T qq;
9240 boolean_T apply_transform;
9241 int32_T qjj;
9242 int32_T m;
9243 int32_T kase;
9244 int32_T c_q;
9245 int32_T d_k;
9246 boolean_T exitg1;
9247 localB->e_i[0] = 0.0;
9248 localB->work[0] = 0.0;
9249 localB->e_i[1] = 0.0;
9250 localB->work[1] = 0.0;
9251 localB->e_i[2] = 0.0;
9252 localB->work[2] = 0.0;
9253 for (m = 0; m < 9; m++) {
9254 localB->A[m] = A[m];
9255 U[m] = 0.0;
9256 V[m] = 0.0;
9257 }
9258
9259 apply_transform = false;
9260 localB->nrm = cartesian_trajectory_plan_xnrm2(3, localB->A, 1);
9261 if (localB->nrm > 0.0) {
9262 apply_transform = true;
9263 if (localB->A[0] < 0.0) {
9264 localB->s[0] = -localB->nrm;
9265 } else {
9266 localB->s[0] = localB->nrm;
9267 }
9268
9269 if (fabs(localB->s[0]) >= 1.0020841800044864E-292) {
9270 localB->nrm = 1.0 / localB->s[0];
9271 for (qq = 1; qq < 4; qq++) {
9272 localB->A[qq - 1] *= localB->nrm;
9273 }
9274 } else {
9275 for (qq = 1; qq < 4; qq++) {
9276 localB->A[qq - 1] /= localB->s[0];
9277 }
9278 }
9279
9280 localB->A[0]++;
9281 localB->s[0] = -localB->s[0];
9282 } else {
9283 localB->s[0] = 0.0;
9284 }
9285
9286 for (m = 2; m < 4; m++) {
9287 qjj = (m - 1) * 3 + 1;
9288 if (apply_transform) {
9289 memcpy(&localB->A_o[0], &localB->A[0], 9U * sizeof(real_T));
9290 cartesian_trajectory_plan_xaxpy(3, -(cartesian_trajectory_plan_xdotc(3,
9291 localB->A, 1, localB->A, qjj) / localB->A[0]), 1, localB->A_o, qjj,
9292 localB->A);
9293 }
9294
9295 localB->e_i[m - 1] = localB->A[qjj - 1];
9296 }
9297
9298 for (m = 1; m < 4; m++) {
9299 U[m - 1] = localB->A[m - 1];
9300 }
9301
9302 localB->nrm = cartesian_trajectory_pl_xnrm2_a(localB->e_i, 2);
9303 if (localB->nrm == 0.0) {
9304 localB->e_i[0] = 0.0;
9305 } else {
9306 if (localB->e_i[1] < 0.0) {
9307 localB->rt = -localB->nrm;
9308 localB->e_i[0] = -localB->nrm;
9309 } else {
9310 localB->rt = localB->nrm;
9311 localB->e_i[0] = localB->nrm;
9312 }
9313
9314 if (fabs(localB->rt) >= 1.0020841800044864E-292) {
9315 localB->nrm = 1.0 / localB->rt;
9316 for (qq = 2; qq < 4; qq++) {
9317 localB->e_i[qq - 1] *= localB->nrm;
9318 }
9319 } else {
9320 for (qq = 2; qq < 4; qq++) {
9321 localB->e_i[qq - 1] /= localB->rt;
9322 }
9323 }
9324
9325 localB->e_i[1]++;
9326 localB->e_i[0] = -localB->e_i[0];
9327 for (m = 2; m < 4; m++) {
9328 localB->work[m - 1] = 0.0;
9329 }
9330
9331 for (m = 2; m < 4; m++) {
9332 cartesian_trajectory__xaxpy_cxo(2, localB->e_i[m - 1], localB->A, 3 * (m -
9333 1) + 2, localB->work, 2);
9334 }
9335
9336 for (m = 2; m < 4; m++) {
9337 memcpy(&localB->A_o[0], &localB->A[0], 9U * sizeof(real_T));
9338 cartesian_trajectory_p_xaxpy_cx(2, -localB->e_i[m - 1] / localB->e_i[1],
9339 localB->work, 2, localB->A_o, (m - 1) * 3 + 2, localB->A);
9340 }
9341 }
9342
9343 for (m = 2; m < 4; m++) {
9344 V[m - 1] = localB->e_i[m - 1];
9345 }
9346
9347 apply_transform = false;
9348 localB->nrm = cartesian_trajectory_plan_xnrm2(2, localB->A, 5);
9349 if (localB->nrm > 0.0) {
9350 apply_transform = true;
9351 if (localB->A[4] < 0.0) {
9352 localB->s[1] = -localB->nrm;
9353 } else {
9354 localB->s[1] = localB->nrm;
9355 }
9356
9357 if (fabs(localB->s[1]) >= 1.0020841800044864E-292) {
9358 localB->nrm = 1.0 / localB->s[1];
9359 for (qq = 5; qq < 7; qq++) {
9360 localB->A[qq - 1] *= localB->nrm;
9361 }
9362 } else {
9363 for (qq = 5; qq < 7; qq++) {
9364 localB->A[qq - 1] /= localB->s[1];
9365 }
9366 }
9367
9368 localB->A[4]++;
9369 localB->s[1] = -localB->s[1];
9370 } else {
9371 localB->s[1] = 0.0;
9372 }
9373
9374 if (apply_transform) {
9375 for (m = 3; m < 4; m++) {
9376 memcpy(&localB->A_o[0], &localB->A[0], 9U * sizeof(real_T));
9377 cartesian_trajectory_plan_xaxpy(2, -(cartesian_trajectory_plan_xdotc(2,
9378 localB->A, 5, localB->A, 8) / localB->A[4]), 5, localB->A_o, 8,
9379 localB->A);
9380 }
9381 }
9382
9383 for (m = 2; m < 4; m++) {
9384 U[m + 2] = localB->A[m + 2];
9385 }
9386
9387 m = 2;
9388 localB->s[2] = localB->A[8];
9389 localB->e_i[1] = localB->A[7];
9390 localB->e_i[2] = 0.0;
9391 U[6] = 0.0;
9392 U[7] = 0.0;
9393 U[8] = 1.0;
9394 for (c_q = 1; c_q >= 0; c_q--) {
9395 qq = 3 * c_q + c_q;
9396 if (localB->s[c_q] != 0.0) {
9397 for (kase = c_q + 2; kase < 4; kase++) {
9398 qjj = ((kase - 1) * 3 + c_q) + 1;
9399 memcpy(&localB->A[0], &U[0], 9U * sizeof(real_T));
9400 cartesian_trajectory_plan_xaxpy(3 - c_q,
9401 -(cartesian_trajectory_plan_xdotc(3 - c_q, U, qq + 1, U, qjj) / U[qq]),
9402 qq + 1, localB->A, qjj, U);
9403 }
9404
9405 for (qjj = c_q + 1; qjj < 4; qjj++) {
9406 kase = (3 * c_q + qjj) - 1;
9407 U[kase] = -U[kase];
9408 }
9409
9410 U[qq]++;
9411 if (0 <= c_q - 1) {
9412 U[3 * c_q] = 0.0;
9413 }
9414 } else {
9415 U[3 * c_q] = 0.0;
9416 U[3 * c_q + 1] = 0.0;
9417 U[3 * c_q + 2] = 0.0;
9418 U[qq] = 1.0;
9419 }
9420 }
9421
9422 for (c_q = 2; c_q >= 0; c_q--) {
9423 if ((c_q + 1 <= 1) && (localB->e_i[0] != 0.0)) {
9424 memcpy(&localB->A[0], &V[0], 9U * sizeof(real_T));
9425 cartesian_trajectory_plan_xaxpy(2, -(cartesian_trajectory_plan_xdotc(2, V,
9426 2, V, 5) / V[1]), 2, localB->A, 5, V);
9427 memcpy(&localB->A[0], &V[0], 9U * sizeof(real_T));
9428 cartesian_trajectory_plan_xaxpy(2, -(cartesian_trajectory_plan_xdotc(2, V,
9429 2, V, 8) / V[1]), 2, localB->A, 8, V);
9430 }
9431
9432 V[3 * c_q] = 0.0;
9433 V[3 * c_q + 1] = 0.0;
9434 V[3 * c_q + 2] = 0.0;
9435 V[c_q + 3 * c_q] = 1.0;
9436 }
9437
9438 for (c_q = 0; c_q < 3; c_q++) {
9439 localB->ztest = localB->e_i[c_q];
9440 if (localB->s[c_q] != 0.0) {
9441 localB->rt = fabs(localB->s[c_q]);
9442 localB->nrm = localB->s[c_q] / localB->rt;
9443 localB->s[c_q] = localB->rt;
9444 if (c_q + 1 < 3) {
9445 localB->ztest = localB->e_i[c_q] / localB->nrm;
9446 }
9447
9448 qjj = 3 * c_q;
9449 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
9450 U[qq - 1] *= localB->nrm;
9451 }
9452 }
9453
9454 if ((c_q + 1 < 3) && (localB->ztest != 0.0)) {
9455 localB->rt = fabs(localB->ztest);
9456 localB->nrm = localB->rt / localB->ztest;
9457 localB->ztest = localB->rt;
9458 localB->s[c_q + 1] *= localB->nrm;
9459 qjj = (c_q + 1) * 3;
9460 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
9461 V[qq - 1] *= localB->nrm;
9462 }
9463 }
9464
9465 localB->e_i[c_q] = localB->ztest;
9466 }
9467
9468 qq = 0;
9469 localB->nrm = 0.0;
9470 localB->ztest = fabs(localB->s[0]);
9471 localB->rt = fabs(localB->e_i[0]);
9472 if ((localB->ztest > localB->rt) || rtIsNaN(localB->rt)) {
9473 localB->rt = localB->ztest;
9474 }
9475
9476 if (!rtIsNaN(localB->rt)) {
9477 localB->nrm = localB->rt;
9478 }
9479
9480 localB->ztest = fabs(localB->s[1]);
9481 localB->rt = fabs(localB->e_i[1]);
9482 if ((localB->ztest > localB->rt) || rtIsNaN(localB->rt)) {
9483 localB->rt = localB->ztest;
9484 }
9485
9486 if ((!(localB->nrm > localB->rt)) && (!rtIsNaN(localB->rt))) {
9487 localB->nrm = localB->rt;
9488 }
9489
9490 localB->ztest = fabs(localB->s[2]);
9491 localB->rt = fabs(localB->e_i[2]);
9492 if ((localB->ztest > localB->rt) || rtIsNaN(localB->rt)) {
9493 localB->rt = localB->ztest;
9494 }
9495
9496 if ((!(localB->nrm > localB->rt)) && (!rtIsNaN(localB->rt))) {
9497 localB->nrm = localB->rt;
9498 }
9499
9500 while ((m + 1 > 0) && (!(qq >= 75))) {
9501 c_q = m;
9502 qjj = m;
9503 exitg1 = false;
9504 while ((!exitg1) && (qjj > -1)) {
9505 c_q = qjj;
9506 if (qjj == 0) {
9507 exitg1 = true;
9508 } else {
9509 localB->rt = fabs(localB->e_i[qjj - 1]);
9510 if ((localB->rt <= (fabs(localB->s[qjj - 1]) + fabs(localB->s[qjj])) *
9511 2.2204460492503131E-16) || (localB->rt <= 1.0020841800044864E-292) ||
9512 ((qq > 20) && (localB->rt <= 2.2204460492503131E-16 * localB->nrm)))
9513 {
9514 localB->e_i[qjj - 1] = 0.0;
9515 exitg1 = true;
9516 } else {
9517 qjj--;
9518 }
9519 }
9520 }
9521
9522 if (c_q == m) {
9523 kase = 4;
9524 } else {
9525 qjj = m + 1;
9526 kase = m + 1;
9527 exitg1 = false;
9528 while ((!exitg1) && (kase >= c_q)) {
9529 qjj = kase;
9530 if (kase == c_q) {
9531 exitg1 = true;
9532 } else {
9533 localB->rt = 0.0;
9534 if (kase < m + 1) {
9535 localB->rt = fabs(localB->e_i[kase - 1]);
9536 }
9537
9538 if (kase > c_q + 1) {
9539 localB->rt += fabs(localB->e_i[kase - 2]);
9540 }
9541
9542 localB->ztest = fabs(localB->s[kase - 1]);
9543 if ((localB->ztest <= 2.2204460492503131E-16 * localB->rt) ||
9544 (localB->ztest <= 1.0020841800044864E-292)) {
9545 localB->s[kase - 1] = 0.0;
9546 exitg1 = true;
9547 } else {
9548 kase--;
9549 }
9550 }
9551 }
9552
9553 if (qjj == c_q) {
9554 kase = 3;
9555 } else if (m + 1 == qjj) {
9556 kase = 1;
9557 } else {
9558 kase = 2;
9559 c_q = qjj;
9560 }
9561 }
9562
9563 switch (kase) {
9564 case 1:
9565 localB->rt = localB->e_i[m - 1];
9566 localB->e_i[m - 1] = 0.0;
9567 for (qjj = m; qjj >= c_q + 1; qjj--) {
9568 localB->ztest = localB->e_i[0];
9569 cartesian_trajectory_plan_xrotg(localB->s[qjj - 1], localB->rt,
9570 &localB->s[qjj - 1], &localB->rt, &localB->sqds, &localB->b_pr, localB);
9571 if (qjj > c_q + 1) {
9572 localB->rt = -localB->b_pr * localB->e_i[0];
9573 localB->ztest = localB->e_i[0] * localB->sqds;
9574 }
9575
9576 memcpy(&localB->A[0], &V[0], 9U * sizeof(real_T));
9577 cartesian_trajectory_plann_xrot(localB->A, (qjj - 1) * 3 + 1, 3 * m + 1,
9578 localB->sqds, localB->b_pr, V);
9579 localB->e_i[0] = localB->ztest;
9580 }
9581 break;
9582
9583 case 2:
9584 localB->rt = localB->e_i[c_q - 1];
9585 localB->e_i[c_q - 1] = 0.0;
9586 for (qjj = c_q + 1; qjj <= m + 1; qjj++) {
9587 cartesian_trajectory_plan_xrotg(localB->s[qjj - 1], localB->rt,
9588 &localB->s[qjj - 1], &localB->ztest, &localB->sqds, &localB->b_pr,
9589 localB);
9590 localB->ztest = localB->e_i[qjj - 1];
9591 localB->rt = localB->ztest * -localB->b_pr;
9592 localB->e_i[qjj - 1] = localB->ztest * localB->sqds;
9593 memcpy(&localB->A[0], &U[0], 9U * sizeof(real_T));
9594 cartesian_trajectory_plann_xrot(localB->A, (qjj - 1) * 3 + 1, (c_q - 1) *
9595 3 + 1, localB->sqds, localB->b_pr, U);
9596 }
9597 break;
9598
9599 case 3:
9600 localB->ztest = fabs(localB->s[m]);
9601 localB->sqds = localB->s[m - 1];
9602 localB->rt = fabs(localB->sqds);
9603 if ((localB->ztest > localB->rt) || rtIsNaN(localB->rt)) {
9604 localB->rt = localB->ztest;
9605 }
9606
9607 localB->b_pr = localB->e_i[m - 1];
9608 localB->ztest = fabs(localB->b_pr);
9609 if ((localB->rt > localB->ztest) || rtIsNaN(localB->ztest)) {
9610 localB->ztest = localB->rt;
9611 }
9612
9613 localB->rt = fabs(localB->s[c_q]);
9614 if ((localB->ztest > localB->rt) || rtIsNaN(localB->rt)) {
9615 localB->rt = localB->ztest;
9616 }
9617
9618 localB->ztest = fabs(localB->e_i[c_q]);
9619 if ((localB->rt > localB->ztest) || rtIsNaN(localB->ztest)) {
9620 localB->ztest = localB->rt;
9621 }
9622
9623 localB->rt = localB->s[m] / localB->ztest;
9624 localB->smm1 = localB->sqds / localB->ztest;
9625 localB->emm1 = localB->b_pr / localB->ztest;
9626 localB->sqds = localB->s[c_q] / localB->ztest;
9627 localB->b_pr = ((localB->smm1 + localB->rt) * (localB->smm1 - localB->rt)
9628 + localB->emm1 * localB->emm1) / 2.0;
9629 localB->smm1 = localB->rt * localB->emm1;
9630 localB->smm1 *= localB->smm1;
9631 if ((localB->b_pr != 0.0) || (localB->smm1 != 0.0)) {
9632 localB->emm1 = sqrt(localB->b_pr * localB->b_pr + localB->smm1);
9633 if (localB->b_pr < 0.0) {
9634 localB->emm1 = -localB->emm1;
9635 }
9636
9637 localB->emm1 = localB->smm1 / (localB->b_pr + localB->emm1);
9638 } else {
9639 localB->emm1 = 0.0;
9640 }
9641
9642 localB->rt = (localB->sqds + localB->rt) * (localB->sqds - localB->rt) +
9643 localB->emm1;
9644 localB->sqds *= localB->e_i[c_q] / localB->ztest;
9645 for (d_k = c_q + 1; d_k <= m; d_k++) {
9646 cartesian_trajectory_plan_xrotg(localB->rt, localB->sqds, &localB->ztest,
9647 &localB->emm1, &localB->b_pr, &localB->smm1, localB);
9648 if (d_k > c_q + 1) {
9649 localB->e_i[0] = localB->ztest;
9650 }
9651
9652 localB->ztest = localB->e_i[d_k - 1];
9653 localB->rt = localB->s[d_k - 1];
9654 localB->e_i[d_k - 1] = localB->ztest * localB->b_pr - localB->rt *
9655 localB->smm1;
9656 localB->sqds = localB->smm1 * localB->s[d_k];
9657 localB->s[d_k] *= localB->b_pr;
9658 qjj = (d_k - 1) * 3 + 1;
9659 kase = 3 * d_k + 1;
9660 memcpy(&localB->A[0], &V[0], 9U * sizeof(real_T));
9661 cartesian_trajectory_plann_xrot(localB->A, qjj, kase, localB->b_pr,
9662 localB->smm1, V);
9663 cartesian_trajectory_plan_xrotg(localB->rt * localB->b_pr +
9664 localB->ztest * localB->smm1, localB->sqds, &localB->s[d_k - 1],
9665 &localB->unusedU2, &localB->emm1, &localB->d_sn, localB);
9666 localB->rt = localB->e_i[d_k - 1] * localB->emm1 + localB->d_sn *
9667 localB->s[d_k];
9668 localB->s[d_k] = localB->e_i[d_k - 1] * -localB->d_sn + localB->emm1 *
9669 localB->s[d_k];
9670 localB->sqds = localB->d_sn * localB->e_i[d_k];
9671 localB->e_i[d_k] *= localB->emm1;
9672 memcpy(&localB->A[0], &U[0], 9U * sizeof(real_T));
9673 cartesian_trajectory_plann_xrot(localB->A, qjj, kase, localB->emm1,
9674 localB->d_sn, U);
9675 }
9676
9677 localB->e_i[m - 1] = localB->rt;
9678 qq++;
9679 break;
9680
9681 default:
9682 if (localB->s[c_q] < 0.0) {
9683 localB->s[c_q] = -localB->s[c_q];
9684 qjj = 3 * c_q;
9685 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
9686 V[qq - 1] = -V[qq - 1];
9687 }
9688 }
9689
9690 qq = c_q + 1;
9691 while ((c_q + 1 < 3) && (localB->s[c_q] < localB->s[qq])) {
9692 localB->rt = localB->s[c_q];
9693 localB->s[c_q] = localB->s[qq];
9694 localB->s[qq] = localB->rt;
9695 qjj = 3 * c_q + 1;
9696 kase = (c_q + 1) * 3 + 1;
9697 memcpy(&localB->A[0], &V[0], 9U * sizeof(real_T));
9698 cartesian_trajectory_plan_xswap(localB->A, qjj, kase, V);
9699 memcpy(&localB->A[0], &U[0], 9U * sizeof(real_T));
9700 cartesian_trajectory_plan_xswap(localB->A, qjj, kase, U);
9701 c_q = qq;
9702 qq++;
9703 }
9704
9705 qq = 0;
9706 m--;
9707 break;
9708 }
9709 }
9710
9711 s[0] = localB->s[0];
9712 s[1] = localB->s[1];
9713 s[2] = localB->s[2];
9714}
9715
9716static void cartesian_trajectory_rotm2axang(const real_T R[9], real_T axang[4],
9717 B_MATLABSystem_cartesian_traj_T *localB)
9718{
9719 boolean_T e;
9720 boolean_T p;
9721 boolean_T rEQ0;
9722 int32_T loop_ub_tmp;
9723 boolean_T exitg1;
9724 localB->u.re = (((R[0] + R[4]) + R[8]) - 1.0) * 0.5;
9725 if (!(fabs(localB->u.re) > 1.0)) {
9726 localB->v_ox.re = acos(localB->u.re);
9727 } else {
9728 localB->u_k.re = localB->u.re + 1.0;
9729 localB->u_k.im = 0.0;
9730 localB->dc.re = 1.0 - localB->u.re;
9731 localB->dc.im = 0.0;
9732 localB->v_ox.re = 2.0 * rt_atan2d_snf((cartesian_trajectory_plann_sqrt
9733 (localB->dc)).re, (cartesian_trajectory_plann_sqrt(localB->u_k)).re);
9734 }
9735
9736 localB->a_m = 2.0 * sin(localB->v_ox.re);
9737 localB->v_f[0] = (R[5] - R[7]) / localB->a_m;
9738 localB->v_f[1] = (R[6] - R[2]) / localB->a_m;
9739 localB->v_f[2] = (R[1] - R[3]) / localB->a_m;
9740 if (rtIsNaN(localB->v_ox.re) || rtIsInf(localB->v_ox.re)) {
9741 localB->a_m = (rtNaN);
9742 } else if (localB->v_ox.re == 0.0) {
9743 localB->a_m = 0.0;
9744 } else {
9745 localB->a_m = fmod(localB->v_ox.re, 3.1415926535897931);
9746 rEQ0 = (localB->a_m == 0.0);
9747 if (!rEQ0) {
9748 localB->q = fabs(localB->v_ox.re / 3.1415926535897931);
9749 rEQ0 = !(fabs(localB->q - floor(localB->q + 0.5)) > 2.2204460492503131E-16
9750 * localB->q);
9751 }
9752
9753 if (rEQ0) {
9754 localB->a_m = 0.0;
9755 } else {
9756 if (localB->v_ox.re < 0.0) {
9757 localB->a_m += 3.1415926535897931;
9758 }
9759 }
9760 }
9761
9762 rEQ0 = (localB->a_m == 0.0);
9763 e = true;
9764 localB->b_k_b = 0;
9765 exitg1 = false;
9766 while ((!exitg1) && (localB->b_k_b < 3)) {
9767 if (!(localB->v_f[localB->b_k_b] == 0.0)) {
9768 e = false;
9769 exitg1 = true;
9770 } else {
9771 localB->b_k_b++;
9772 }
9773 }
9774
9775 if (rEQ0 || e) {
9776 loop_ub_tmp = (rEQ0 || e);
9777 localB->loop_ub_os = loop_ub_tmp * 3 - 1;
9778 if (0 <= localB->loop_ub_os) {
9779 memset(&localB->vspecial_data[0], 0, (localB->loop_ub_os + 1) * sizeof
9780 (real_T));
9781 }
9782
9783 loop_ub_tmp--;
9784 for (localB->loop_ub_os = 0; localB->loop_ub_os <= loop_ub_tmp;
9785 localB->loop_ub_os++) {
9786 memset(&localB->b_I[0], 0, 9U * sizeof(real_T));
9787 localB->b_I[0] = 1.0;
9788 localB->b_I[4] = 1.0;
9789 localB->b_I[8] = 1.0;
9790 p = true;
9791 for (localB->b_k_b = 0; localB->b_k_b < 9; localB->b_k_b++) {
9792 localB->a_m = localB->b_I[localB->b_k_b] - R[localB->b_k_b];
9793 if (p && ((!rtIsInf(localB->a_m)) && (!rtIsNaN(localB->a_m)))) {
9794 } else {
9795 p = false;
9796 }
9797
9798 localB->b_I[localB->b_k_b] = localB->a_m;
9799 }
9800
9801 if (p) {
9802 cartesian_trajectory_planne_svd(localB->b_I, localB->b_U,
9803 localB->vspecial_data, localB->V_o, localB);
9804 } else {
9805 for (localB->b_k_b = 0; localB->b_k_b < 9; localB->b_k_b++) {
9806 localB->V_o[localB->b_k_b] = (rtNaN);
9807 }
9808 }
9809
9810 localB->vspecial_data[0] = localB->V_o[6];
9811 localB->vspecial_data[1] = localB->V_o[7];
9812 localB->vspecial_data[2] = localB->V_o[8];
9813 }
9814
9815 loop_ub_tmp = 0;
9816 if (rEQ0 || e) {
9817 for (localB->loop_ub_os = 0; localB->loop_ub_os < 1; localB->loop_ub_os++)
9818 {
9819 loop_ub_tmp++;
9820 }
9821 }
9822
9823 for (localB->b_k_b = 0; localB->b_k_b < loop_ub_tmp; localB->b_k_b++) {
9824 localB->v_f[0] = localB->vspecial_data[3 * localB->b_k_b];
9825 localB->v_f[1] = localB->vspecial_data[3 * localB->b_k_b + 1];
9826 localB->v_f[2] = localB->vspecial_data[3 * localB->b_k_b + 2];
9827 }
9828 }
9829
9830 localB->a_m = 1.0 / sqrt((localB->v_f[0] * localB->v_f[0] + localB->v_f[1] *
9831 localB->v_f[1]) + localB->v_f[2] * localB->v_f[2]);
9832 localB->v_f[0] *= localB->a_m;
9833 localB->v_f[1] *= localB->a_m;
9834 axang[0] = localB->v_f[0];
9835 axang[1] = localB->v_f[1];
9836 axang[2] = localB->v_f[2] * localB->a_m;
9837 axang[3] = localB->v_ox.re;
9838}
9839
9840static void cartesian_IKHelpers_computeCost(const real_T x[6],
9841 f_robotics_manip_internal_IKE_T *args, real_T *cost, real_T W[36],
9842 emxArray_real_T_cartesian_tra_T *Jac, f_robotics_manip_internal_IKE_T **b_args,
9843 B_MATLABSystem_cartesian_traj_T *localB)
9844{
9845 x_robotics_manip_internal_Rig_T *treeInternal;
9846 emxArray_char_T_cartesian_tra_T *bodyName;
9847 emxArray_real_T_cartesian_tra_T *J;
9848 emxArray_real_T_cartesian_tra_T *y;
9849 cartesian_trajec_emxInit_char_T(&bodyName, 2, localB);
9850 *b_args = args;
9851 treeInternal = args->Robot;
9852 localB->b_j_a = bodyName->size[0] * bodyName->size[1];
9853 bodyName->size[0] = 1;
9854 bodyName->size[1] = args->BodyName->size[1];
9855 cartes_emxEnsureCapacity_char_T(bodyName, localB->b_j_a, localB);
9856 localB->loop_ub_d = args->BodyName->size[0] * args->BodyName->size[1] - 1;
9857 for (localB->b_j_a = 0; localB->b_j_a <= localB->loop_ub_d; localB->b_j_a++) {
9858 bodyName->data[localB->b_j_a] = args->BodyName->data[localB->b_j_a];
9859 }
9860
9861 for (localB->b_j_a = 0; localB->b_j_a < 16; localB->b_j_a++) {
9862 localB->Td[localB->b_j_a] = args->Tform[localB->b_j_a];
9863 }
9864
9865 for (localB->b_j_a = 0; localB->b_j_a < 36; localB->b_j_a++) {
9866 W[localB->b_j_a] = args->WeightMatrix[localB->b_j_a];
9867 }
9868
9869 cartesian_trajec_emxInit_real_T(&J, 2, localB);
9870 RigidBodyTree_efficientFKAndJac(treeInternal, x, bodyName, localB->T_data,
9871 localB->T_size, J, localB);
9872 localB->b_j_a = Jac->size[0] * Jac->size[1];
9873 Jac->size[0] = 6;
9874 Jac->size[1] = J->size[1];
9875 cartes_emxEnsureCapacity_real_T(Jac, localB->b_j_a, localB);
9876 localB->loop_ub_d = J->size[0] * J->size[1] - 1;
9877 cartesian_trajec_emxFree_char_T(&bodyName);
9878 for (localB->b_j_a = 0; localB->b_j_a <= localB->loop_ub_d; localB->b_j_a++) {
9879 Jac->data[localB->b_j_a] = -J->data[localB->b_j_a];
9880 }
9881
9882 cartesian_trajec_emxFree_real_T(&J);
9883 for (localB->b_j_a = 0; localB->b_j_a < 3; localB->b_j_a++) {
9884 localB->T[3 * localB->b_j_a] = localB->T_data[localB->b_j_a];
9885 localB->n_k = 3 * localB->b_j_a + 1;
9886 localB->T[localB->n_k] = localB->T_data[((localB->b_j_a + 1) +
9887 localB->T_size[0]) - 1];
9888 localB->boffset_f = 3 * localB->b_j_a + 2;
9889 localB->T[localB->boffset_f] = localB->T_data[((localB->b_j_a + 1) +
9890 (localB->T_size[0] << 1)) - 1];
9891 for (localB->loop_ub_d = 0; localB->loop_ub_d < 3; localB->loop_ub_d++) {
9892 localB->Td_tmp = localB->loop_ub_d + 3 * localB->b_j_a;
9893 localB->Td_p[localB->Td_tmp] = 0.0;
9894 localB->Td_p[localB->Td_tmp] += localB->T[3 * localB->b_j_a] * localB->
9895 Td[localB->loop_ub_d];
9896 localB->Td_p[localB->Td_tmp] += localB->T[localB->n_k] * localB->Td
9897 [localB->loop_ub_d + 4];
9898 localB->Td_p[localB->Td_tmp] += localB->T[localB->boffset_f] * localB->
9899 Td[localB->loop_ub_d + 8];
9900 }
9901 }
9902
9903 cartesian_trajectory_rotm2axang(localB->Td_p, localB->v, localB);
9904 localB->e[0] = localB->v[3] * localB->v[0];
9905 localB->e[3] = localB->Td[12] - localB->T_data[localB->T_size[0] * 3];
9906 localB->e[1] = localB->v[3] * localB->v[1];
9907 localB->e[4] = localB->Td[13] - localB->T_data[localB->T_size[0] * 3 + 1];
9908 localB->e[2] = localB->v[3] * localB->v[2];
9909 localB->e[5] = localB->Td[14] - localB->T_data[localB->T_size[0] * 3 + 2];
9910 localB->b_j_a = args->ErrTemp->size[0];
9911 args->ErrTemp->size[0] = 6;
9912 cartes_emxEnsureCapacity_real_T(args->ErrTemp, localB->b_j_a, localB);
9913 for (localB->b_j_a = 0; localB->b_j_a < 6; localB->b_j_a++) {
9914 args->ErrTemp->data[localB->b_j_a] = localB->e[localB->b_j_a];
9915 }
9916
9917 for (localB->b_j_a = 0; localB->b_j_a < 6; localB->b_j_a++) {
9918 localB->y[localB->b_j_a] = 0.0;
9919 for (localB->loop_ub_d = 0; localB->loop_ub_d < 6; localB->loop_ub_d++) {
9920 localB->s_g = W[6 * localB->b_j_a + localB->loop_ub_d] * (0.5 * localB->
9921 e[localB->loop_ub_d]) + localB->y[localB->b_j_a];
9922 localB->y[localB->b_j_a] = localB->s_g;
9923 }
9924 }
9925
9926 localB->s_g = 0.0;
9927 for (localB->b_j_a = 0; localB->b_j_a < 6; localB->b_j_a++) {
9928 localB->s_g += localB->y[localB->b_j_a] * localB->e[localB->b_j_a];
9929 }
9930
9931 args->CostTemp = localB->s_g;
9932 for (localB->b_j_a = 0; localB->b_j_a < 6; localB->b_j_a++) {
9933 localB->y[localB->b_j_a] = 0.0;
9934 for (localB->loop_ub_d = 0; localB->loop_ub_d < 6; localB->loop_ub_d++) {
9935 localB->s_g = W[6 * localB->b_j_a + localB->loop_ub_d] * localB->e
9936 [localB->loop_ub_d] + localB->y[localB->b_j_a];
9937 localB->y[localB->b_j_a] = localB->s_g;
9938 }
9939 }
9940
9941 cartesian_trajec_emxInit_real_T(&y, 2, localB);
9942 localB->n_k = Jac->size[1] - 1;
9943 localB->b_j_a = y->size[0] * y->size[1];
9944 y->size[0] = 1;
9945 y->size[1] = Jac->size[1];
9946 cartes_emxEnsureCapacity_real_T(y, localB->b_j_a, localB);
9947 for (localB->b_j_a = 0; localB->b_j_a <= localB->n_k; localB->b_j_a++) {
9948 localB->boffset_f = localB->b_j_a * 6 - 1;
9949 localB->s_g = 0.0;
9950 for (localB->loop_ub_d = 0; localB->loop_ub_d < 6; localB->loop_ub_d++) {
9951 localB->s_g += Jac->data[(localB->boffset_f + localB->loop_ub_d) + 1] *
9952 localB->y[localB->loop_ub_d];
9953 }
9954
9955 y->data[localB->b_j_a] = localB->s_g;
9956 }
9957
9958 localB->b_j_a = args->GradTemp->size[0];
9959 args->GradTemp->size[0] = y->size[1];
9960 cartes_emxEnsureCapacity_real_T(args->GradTemp, localB->b_j_a, localB);
9961 localB->loop_ub_d = y->size[1];
9962 for (localB->b_j_a = 0; localB->b_j_a < localB->loop_ub_d; localB->b_j_a++) {
9963 args->GradTemp->data[localB->b_j_a] = y->data[localB->b_j_a];
9964 }
9965
9966 cartesian_trajec_emxFree_real_T(&y);
9967 localB->s_g = args->CostTemp;
9968 *cost = localB->s_g;
9969}
9970
9971static void cartesian_trajectory_planne_eye(real_T b_I[36])
9972{
9973 int32_T b_k;
9974 memset(&b_I[0], 0, 36U * sizeof(real_T));
9975 for (b_k = 0; b_k < 6; b_k++) {
9976 b_I[b_k + 6 * b_k] = 1.0;
9977 }
9978}
9979
9980static void cartesian_tra_emxInit_boolean_T(emxArray_boolean_T_cartesian__T
9981 **pEmxArray, int32_T numDimensions)
9982{
9983 emxArray_boolean_T_cartesian__T *emxArray;
9984 int32_T i;
9985 *pEmxArray = (emxArray_boolean_T_cartesian__T *)malloc(sizeof
9986 (emxArray_boolean_T_cartesian__T));
9987 emxArray = *pEmxArray;
9988 emxArray->data = (boolean_T *)NULL;
9989 emxArray->numDimensions = numDimensions;
9990 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
9991 emxArray->allocatedSize = 0;
9992 emxArray->canFreeData = true;
9993 for (i = 0; i < numDimensions; i++) {
9994 emxArray->size[i] = 0;
9995 }
9996}
9997
9998static void cartesian_traje_emxInit_int32_T(emxArray_int32_T_cartesian_tr_T
9999 **pEmxArray, int32_T numDimensions)
10000{
10001 emxArray_int32_T_cartesian_tr_T *emxArray;
10002 int32_T i;
10003 *pEmxArray = (emxArray_int32_T_cartesian_tr_T *)malloc(sizeof
10004 (emxArray_int32_T_cartesian_tr_T));
10005 emxArray = *pEmxArray;
10006 emxArray->data = (int32_T *)NULL;
10007 emxArray->numDimensions = numDimensions;
10008 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
10009 emxArray->allocatedSize = 0;
10010 emxArray->canFreeData = true;
10011 for (i = 0; i < numDimensions; i++) {
10012 emxArray->size[i] = 0;
10013 }
10014}
10015
10016static void car_emxEnsureCapacity_boolean_T(emxArray_boolean_T_cartesian__T
10017 *emxArray, int32_T oldNumel)
10018{
10019 int32_T newNumel;
10020 int32_T i;
10021 void *newData;
10022 if (oldNumel < 0) {
10023 oldNumel = 0;
10024 }
10025
10026 newNumel = 1;
10027 for (i = 0; i < emxArray->numDimensions; i++) {
10028 newNumel *= emxArray->size[i];
10029 }
10030
10031 if (newNumel > emxArray->allocatedSize) {
10032 i = emxArray->allocatedSize;
10033 if (i < 16) {
10034 i = 16;
10035 }
10036
10037 while (i < newNumel) {
10038 if (i > 1073741823) {
10039 i = MAX_int32_T;
10040 } else {
10041 i <<= 1;
10042 }
10043 }
10044
10045 newData = calloc(static_cast<uint32_T>(i), sizeof(boolean_T));
10046 if (emxArray->data != NULL) {
10047 memcpy(newData, emxArray->data, sizeof(boolean_T) * oldNumel);
10048 if (emxArray->canFreeData) {
10049 free(emxArray->data);
10050 }
10051 }
10052
10053 emxArray->data = (boolean_T *)newData;
10054 emxArray->allocatedSize = i;
10055 emxArray->canFreeData = true;
10056 }
10057}
10058
10059static void carte_emxEnsureCapacity_int32_T(emxArray_int32_T_cartesian_tr_T
10060 *emxArray, int32_T oldNumel)
10061{
10062 int32_T newNumel;
10063 int32_T i;
10064 void *newData;
10065 if (oldNumel < 0) {
10066 oldNumel = 0;
10067 }
10068
10069 newNumel = 1;
10070 for (i = 0; i < emxArray->numDimensions; i++) {
10071 newNumel *= emxArray->size[i];
10072 }
10073
10074 if (newNumel > emxArray->allocatedSize) {
10075 i = emxArray->allocatedSize;
10076 if (i < 16) {
10077 i = 16;
10078 }
10079
10080 while (i < newNumel) {
10081 if (i > 1073741823) {
10082 i = MAX_int32_T;
10083 } else {
10084 i <<= 1;
10085 }
10086 }
10087
10088 newData = calloc(static_cast<uint32_T>(i), sizeof(int32_T));
10089 if (emxArray->data != NULL) {
10090 memcpy(newData, emxArray->data, sizeof(int32_T) * oldNumel);
10091 if (emxArray->canFreeData) {
10092 free(emxArray->data);
10093 }
10094 }
10095
10096 emxArray->data = (int32_T *)newData;
10097 emxArray->allocatedSize = i;
10098 emxArray->canFreeData = true;
10099 }
10100}
10101
10102static real_T SystemTimeProvider_getElapsedTi(const
10103 f_robotics_core_internal_Syst_T *obj)
10104{
10105 real_T systemTime;
10106 systemTime = ctimefun();
10107 return systemTime - obj->StartTime;
10108}
10109
10110static real_T cartesian_trajectory_pla_norm_j(const real_T x[6])
10111{
10112 real_T y;
10113 real_T scale;
10114 real_T absxk;
10115 real_T t;
10116 int32_T b_k;
10117 y = 0.0;
10118 scale = 3.3121686421112381E-170;
10119 for (b_k = 0; b_k < 6; b_k++) {
10120 absxk = fabs(x[b_k]);
10121 if (absxk > scale) {
10122 t = scale / absxk;
10123 y = y * t * t + 1.0;
10124 scale = absxk;
10125 } else {
10126 t = absxk / scale;
10127 y += t * t;
10128 }
10129 }
10130
10131 return scale * sqrt(y);
10132}
10133
10134static void cartesian_tra_emxFree_boolean_T(emxArray_boolean_T_cartesian__T
10135 **pEmxArray)
10136{
10137 if (*pEmxArray != (emxArray_boolean_T_cartesian__T *)NULL) {
10138 if (((*pEmxArray)->data != (boolean_T *)NULL) && (*pEmxArray)->canFreeData)
10139 {
10140 free((*pEmxArray)->data);
10141 }
10142
10143 free((*pEmxArray)->size);
10144 free(*pEmxArray);
10145 *pEmxArray = (emxArray_boolean_T_cartesian__T *)NULL;
10146 }
10147}
10148
10149static boolean_T DampedBFGSwGradientProjection_a(const
10150 h_robotics_core_internal_Damp_T *obj, const real_T Hg[6], const
10151 emxArray_real_T_cartesian_tra_T *alpha)
10152{
10153 boolean_T flag;
10154 boolean_T y;
10155 emxArray_boolean_T_cartesian__T *x;
10156 int32_T ix;
10157 int32_T loop_ub;
10158 boolean_T exitg1;
10159 cartesian_tra_emxInit_boolean_T(&x, 1);
10160 if (cartesian_trajectory_pla_norm_j(Hg) < obj->GradientTolerance) {
10161 ix = x->size[0];
10162 x->size[0] = alpha->size[0];
10163 car_emxEnsureCapacity_boolean_T(x, ix);
10164 loop_ub = alpha->size[0];
10165 for (ix = 0; ix < loop_ub; ix++) {
10166 x->data[ix] = (alpha->data[ix] <= 0.0);
10167 }
10168
10169 y = true;
10170 ix = 0;
10171 exitg1 = false;
10172 while ((!exitg1) && (ix + 1 <= x->size[0])) {
10173 if (!x->data[ix]) {
10174 y = false;
10175 exitg1 = true;
10176 } else {
10177 ix++;
10178 }
10179 }
10180
10181 if (y) {
10182 flag = true;
10183 } else {
10184 flag = false;
10185 }
10186 } else {
10187 flag = false;
10188 }
10189
10190 cartesian_tra_emxFree_boolean_T(&x);
10191 return flag;
10192}
10193
10194static void cartesian_traje_emxFree_int32_T(emxArray_int32_T_cartesian_tr_T
10195 **pEmxArray)
10196{
10197 if (*pEmxArray != (emxArray_int32_T_cartesian_tr_T *)NULL) {
10198 if (((*pEmxArray)->data != (int32_T *)NULL) && (*pEmxArray)->canFreeData) {
10199 free((*pEmxArray)->data);
10200 }
10201
10202 free((*pEmxArray)->size);
10203 free(*pEmxArray);
10204 *pEmxArray = (emxArray_int32_T_cartesian_tr_T *)NULL;
10205 }
10206}
10207
10208static void cartesian_trajectory_pl_xzgetrf(int32_T m, int32_T n, const
10209 emxArray_real_T_cartesian_tra_T *A, int32_T lda,
10210 emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T *ipiv,
10211 int32_T *info, B_MATLABSystem_cartesian_traj_T *localB)
10212{
10213 int32_T mmj;
10214 int32_T b;
10215 int32_T c;
10216 emxArray_real_T_cartesian_tra_T *c_x;
10217 int32_T ix;
10218 real_T smax;
10219 real_T s;
10220 int32_T iy;
10221 int32_T n_0;
10222 int32_T yk;
10223 int32_T k;
10224 int32_T jA;
10225 int32_T jy;
10226 int32_T c_0;
10227 int32_T c_tmp;
10228 k = b_A->size[0] * b_A->size[1];
10229 b_A->size[0] = A->size[0];
10230 b_A->size[1] = A->size[1];
10231 cartes_emxEnsureCapacity_real_T(b_A, k, localB);
10232 iy = A->size[0] * A->size[1] - 1;
10233 for (k = 0; k <= iy; k++) {
10234 b_A->data[k] = A->data[k];
10235 }
10236
10237 if (m < n) {
10238 n_0 = m;
10239 } else {
10240 n_0 = n;
10241 }
10242
10243 if (n_0 < 1) {
10244 n_0 = 0;
10245 }
10246
10247 k = ipiv->size[0] * ipiv->size[1];
10248 ipiv->size[0] = 1;
10249 ipiv->size[1] = n_0;
10250 carte_emxEnsureCapacity_int32_T(ipiv, k);
10251 if (n_0 > 0) {
10252 ipiv->data[0] = 1;
10253 yk = 1;
10254 for (k = 2; k <= n_0; k++) {
10255 yk++;
10256 ipiv->data[k - 1] = yk;
10257 }
10258 }
10259
10260 yk = 0;
10261 cartesian_trajec_emxInit_real_T(&c_x, 2, localB);
10262 if ((m < 1) || (n < 1)) {
10263 } else {
10264 n_0 = m - 1;
10265 if (n_0 >= n) {
10266 n_0 = n;
10267 }
10268
10269 b = n_0 - 1;
10270 for (n_0 = 0; n_0 <= b; n_0++) {
10271 mmj = m - n_0;
10272 c_tmp = (lda + 1) * n_0;
10273 c = c_tmp + 2;
10274 if (mmj < 1) {
10275 iy = 0;
10276 } else {
10277 iy = 1;
10278 if (mmj > 1) {
10279 ix = c - 2;
10280 smax = fabs(b_A->data[c_tmp]);
10281 for (k = 2; k <= mmj; k++) {
10282 ix++;
10283 s = fabs(b_A->data[ix]);
10284 if (s > smax) {
10285 iy = k;
10286 smax = s;
10287 }
10288 }
10289 }
10290 }
10291
10292 if (b_A->data[(c + iy) - 3] != 0.0) {
10293 if (iy - 1 != 0) {
10294 k = n_0 + iy;
10295 ipiv->data[n_0] = k;
10296 ix = c_x->size[0] * c_x->size[1];
10297 c_x->size[0] = b_A->size[0];
10298 c_x->size[1] = b_A->size[1];
10299 cartes_emxEnsureCapacity_real_T(c_x, ix, localB);
10300 iy = b_A->size[0] * b_A->size[1] - 1;
10301 for (ix = 0; ix <= iy; ix++) {
10302 c_x->data[ix] = b_A->data[ix];
10303 }
10304
10305 ix = n_0;
10306 iy = k - 1;
10307 for (k = 0; k < n; k++) {
10308 smax = c_x->data[ix];
10309 c_x->data[ix] = c_x->data[iy];
10310 c_x->data[iy] = smax;
10311 ix += lda;
10312 iy += lda;
10313 }
10314
10315 k = b_A->size[0] * b_A->size[1];
10316 b_A->size[0] = c_x->size[0];
10317 b_A->size[1] = c_x->size[1];
10318 cartes_emxEnsureCapacity_real_T(b_A, k, localB);
10319 iy = c_x->size[0] * c_x->size[1] - 1;
10320 for (k = 0; k <= iy; k++) {
10321 b_A->data[k] = c_x->data[k];
10322 }
10323 }
10324
10325 iy = c + mmj;
10326 for (k = c; k <= iy - 2; k++) {
10327 b_A->data[k - 1] /= b_A->data[c_tmp];
10328 }
10329 } else {
10330 yk = n_0 + 1;
10331 }
10332
10333 iy = (n - n_0) - 2;
10334 jy = c_tmp + lda;
10335 jA = jy + 1;
10336 for (k = 0; k <= iy; k++) {
10337 smax = b_A->data[jy];
10338 if (b_A->data[jy] != 0.0) {
10339 ix = c - 1;
10340 c_0 = (mmj + jA) - 1;
10341 for (c_tmp = jA + 1; c_tmp <= c_0; c_tmp++) {
10342 b_A->data[c_tmp - 1] += b_A->data[ix] * -smax;
10343 ix++;
10344 }
10345 }
10346
10347 jy += lda;
10348 jA += lda;
10349 }
10350 }
10351
10352 if ((yk == 0) && (m <= n) && (!(b_A->data[((m - 1) * b_A->size[0] + m) - 1]
10353 != 0.0))) {
10354 yk = m;
10355 }
10356 }
10357
10358 cartesian_trajec_emxFree_real_T(&c_x);
10359 *info = yk;
10360}
10361
10362static void cartesian_trajectory_plan_xtrsm(int32_T m, int32_T n, const
10363 emxArray_real_T_cartesian_tra_T *A, int32_T lda, const
10364 emxArray_real_T_cartesian_tra_T *B, int32_T ldb,
10365 emxArray_real_T_cartesian_tra_T *b_B, B_MATLABSystem_cartesian_traj_T *localB)
10366{
10367 int32_T jBcol;
10368 int32_T kAcol;
10369 int32_T i;
10370 int32_T k;
10371 int32_T b;
10372 int32_T b_i;
10373 int32_T loop_ub;
10374 int32_T i_0;
10375 int32_T tmp;
10376 i_0 = b_B->size[0] * b_B->size[1];
10377 b_B->size[0] = B->size[0];
10378 b_B->size[1] = B->size[1];
10379 cartes_emxEnsureCapacity_real_T(b_B, i_0, localB);
10380 loop_ub = B->size[0] * B->size[1] - 1;
10381 for (i_0 = 0; i_0 <= loop_ub; i_0++) {
10382 b_B->data[i_0] = B->data[i_0];
10383 }
10384
10385 if ((n == 0) || ((B->size[0] == 0) || (B->size[1] == 0))) {
10386 } else {
10387 for (loop_ub = 0; loop_ub < n; loop_ub++) {
10388 jBcol = ldb * loop_ub - 1;
10389 for (k = m; k >= 1; k--) {
10390 kAcol = (k - 1) * lda - 1;
10391 i_0 = k + jBcol;
10392 if (b_B->data[i_0] != 0.0) {
10393 b_B->data[i_0] /= A->data[k + kAcol];
10394 b = k - 2;
10395 for (b_i = 0; b_i <= b; b_i++) {
10396 i = b_i + 1;
10397 tmp = i + jBcol;
10398 b_B->data[tmp] -= b_B->data[i_0] * A->data[i + kAcol];
10399 }
10400 }
10401 }
10402 }
10403 }
10404}
10405
10406static real_T cartesian_trajectory_p_xnrm2_ad(int32_T n, const
10407 emxArray_real_T_cartesian_tra_T *x, int32_T ix0,
10408 B_MATLABSystem_cartesian_traj_T *localB)
10409{
10410 real_T y;
10411 y = 0.0;
10412 if (n >= 1) {
10413 if (n == 1) {
10414 y = fabs(x->data[ix0 - 1]);
10415 } else {
10416 localB->scale_n = 3.3121686421112381E-170;
10417 localB->kend_c = ix0 + n;
10418 for (localB->k_k = ix0; localB->k_k < localB->kend_c; localB->k_k++) {
10419 localB->absxk_p = fabs(x->data[localB->k_k - 1]);
10420 if (localB->absxk_p > localB->scale_n) {
10421 localB->t_d = localB->scale_n / localB->absxk_p;
10422 y = y * localB->t_d * localB->t_d + 1.0;
10423 localB->scale_n = localB->absxk_p;
10424 } else {
10425 localB->t_d = localB->absxk_p / localB->scale_n;
10426 y += localB->t_d * localB->t_d;
10427 }
10428 }
10429
10430 y = localB->scale_n * sqrt(y);
10431 }
10432 }
10433
10434 return y;
10435}
10436
10437static void cartesian_trajectory_pla_qrpf_b(const
10438 emxArray_real_T_cartesian_tra_T *A, int32_T m, int32_T n,
10439 emxArray_real_T_cartesian_tra_T *tau, const emxArray_int32_T_cartesian_tr_T
10440 *jpvt, emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T
10441 *b_jpvt, B_MATLABSystem_cartesian_traj_T *localB)
10442{
10443 emxArray_real_T_cartesian_tra_T *work;
10444 emxArray_real_T_cartesian_tra_T *vn1;
10445 emxArray_real_T_cartesian_tra_T *vn2;
10446 emxArray_real_T_cartesian_tra_T *c_x;
10447 int32_T exitg1;
10448 boolean_T exitg2;
10449 localB->kend = b_jpvt->size[0] * b_jpvt->size[1];
10450 b_jpvt->size[0] = 1;
10451 b_jpvt->size[1] = jpvt->size[1];
10452 carte_emxEnsureCapacity_int32_T(b_jpvt, localB->kend);
10453 localB->ix_i = jpvt->size[0] * jpvt->size[1] - 1;
10454 for (localB->kend = 0; localB->kend <= localB->ix_i; localB->kend++) {
10455 b_jpvt->data[localB->kend] = jpvt->data[localB->kend];
10456 }
10457
10458 localB->kend = b_A->size[0] * b_A->size[1];
10459 b_A->size[0] = A->size[0];
10460 b_A->size[1] = A->size[1];
10461 cartes_emxEnsureCapacity_real_T(b_A, localB->kend, localB);
10462 localB->ix_i = A->size[0] * A->size[1] - 1;
10463 for (localB->kend = 0; localB->kend <= localB->ix_i; localB->kend++) {
10464 b_A->data[localB->kend] = A->data[localB->kend];
10465 }
10466
10467 cartesian_trajec_emxInit_real_T(&work, 1, localB);
10468 localB->ma = A->size[0];
10469 if (m < n) {
10470 localB->m_gr = m;
10471 } else {
10472 localB->m_gr = n;
10473 }
10474
10475 localB->minmn_e = localB->m_gr - 1;
10476 localB->kend = work->size[0];
10477 work->size[0] = A->size[1];
10478 cartes_emxEnsureCapacity_real_T(work, localB->kend, localB);
10479 localB->ix_i = A->size[1];
10480 for (localB->kend = 0; localB->kend < localB->ix_i; localB->kend++) {
10481 work->data[localB->kend] = 0.0;
10482 }
10483
10484 cartesian_trajec_emxInit_real_T(&vn1, 1, localB);
10485 localB->kend = vn1->size[0];
10486 vn1->size[0] = A->size[1];
10487 cartes_emxEnsureCapacity_real_T(vn1, localB->kend, localB);
10488 localB->ix_i = A->size[1];
10489 for (localB->kend = 0; localB->kend < localB->ix_i; localB->kend++) {
10490 vn1->data[localB->kend] = 0.0;
10491 }
10492
10493 cartesian_trajec_emxInit_real_T(&vn2, 1, localB);
10494 localB->kend = vn2->size[0];
10495 vn2->size[0] = A->size[1];
10496 cartes_emxEnsureCapacity_real_T(vn2, localB->kend, localB);
10497 localB->ix_i = A->size[1];
10498 for (localB->kend = 0; localB->kend < localB->ix_i; localB->kend++) {
10499 vn2->data[localB->kend] = 0.0;
10500 }
10501
10502 for (localB->m_gr = 0; localB->m_gr < n; localB->m_gr++) {
10503 localB->pvt = localB->m_gr * localB->ma;
10504 localB->smax = 0.0;
10505 if (m >= 1) {
10506 if (m == 1) {
10507 localB->smax = fabs(A->data[localB->pvt]);
10508 } else {
10509 localB->scale_c = 3.3121686421112381E-170;
10510 localB->kend = localB->pvt + m;
10511 for (localB->itemp = localB->pvt + 1; localB->itemp <= localB->kend;
10512 localB->itemp++) {
10513 localB->absxk = fabs(A->data[localB->itemp - 1]);
10514 if (localB->absxk > localB->scale_c) {
10515 localB->t = localB->scale_c / localB->absxk;
10516 localB->smax = localB->smax * localB->t * localB->t + 1.0;
10517 localB->scale_c = localB->absxk;
10518 } else {
10519 localB->t = localB->absxk / localB->scale_c;
10520 localB->smax += localB->t * localB->t;
10521 }
10522 }
10523
10524 localB->smax = localB->scale_c * sqrt(localB->smax);
10525 }
10526 }
10527
10528 vn1->data[localB->m_gr] = localB->smax;
10529 vn2->data[localB->m_gr] = vn1->data[localB->m_gr];
10530 }
10531
10532 cartesian_trajec_emxInit_real_T(&c_x, 2, localB);
10533 for (localB->m_gr = 0; localB->m_gr <= localB->minmn_e; localB->m_gr++) {
10534 localB->iy_g = localB->m_gr * localB->ma;
10535 localB->ii = localB->iy_g + localB->m_gr;
10536 localB->nmi = n - localB->m_gr;
10537 localB->mmi = (m - localB->m_gr) - 1;
10538 if (localB->nmi < 1) {
10539 localB->kend = 0;
10540 } else {
10541 localB->kend = 1;
10542 if (localB->nmi > 1) {
10543 localB->ix_i = localB->m_gr;
10544 localB->smax = fabs(vn1->data[localB->m_gr]);
10545 for (localB->itemp = 2; localB->itemp <= localB->nmi; localB->itemp++) {
10546 localB->ix_i++;
10547 localB->scale_c = fabs(vn1->data[localB->ix_i]);
10548 if (localB->scale_c > localB->smax) {
10549 localB->kend = localB->itemp;
10550 localB->smax = localB->scale_c;
10551 }
10552 }
10553 }
10554 }
10555
10556 localB->pvt = (localB->m_gr + localB->kend) - 1;
10557 if (localB->pvt + 1 != localB->m_gr + 1) {
10558 localB->kend = c_x->size[0] * c_x->size[1];
10559 c_x->size[0] = b_A->size[0];
10560 c_x->size[1] = b_A->size[1];
10561 cartes_emxEnsureCapacity_real_T(c_x, localB->kend, localB);
10562 localB->ix_i = b_A->size[0] * b_A->size[1] - 1;
10563 for (localB->kend = 0; localB->kend <= localB->ix_i; localB->kend++) {
10564 c_x->data[localB->kend] = b_A->data[localB->kend];
10565 }
10566
10567 localB->ix_i = localB->pvt * localB->ma;
10568 for (localB->itemp = 0; localB->itemp < m; localB->itemp++) {
10569 localB->scale_c = c_x->data[localB->ix_i];
10570 c_x->data[localB->ix_i] = c_x->data[localB->iy_g];
10571 c_x->data[localB->iy_g] = localB->scale_c;
10572 localB->ix_i++;
10573 localB->iy_g++;
10574 }
10575
10576 localB->kend = b_A->size[0] * b_A->size[1];
10577 b_A->size[0] = c_x->size[0];
10578 b_A->size[1] = c_x->size[1];
10579 cartes_emxEnsureCapacity_real_T(b_A, localB->kend, localB);
10580 localB->ix_i = c_x->size[0] * c_x->size[1] - 1;
10581 for (localB->kend = 0; localB->kend <= localB->ix_i; localB->kend++) {
10582 b_A->data[localB->kend] = c_x->data[localB->kend];
10583 }
10584
10585 localB->itemp = b_jpvt->data[localB->pvt];
10586 b_jpvt->data[localB->pvt] = b_jpvt->data[localB->m_gr];
10587 b_jpvt->data[localB->m_gr] = localB->itemp;
10588 vn1->data[localB->pvt] = vn1->data[localB->m_gr];
10589 vn2->data[localB->pvt] = vn2->data[localB->m_gr];
10590 }
10591
10592 if (localB->m_gr + 1 < m) {
10593 localB->pvt = localB->ii + 2;
10594 localB->kend = c_x->size[0] * c_x->size[1];
10595 c_x->size[0] = b_A->size[0];
10596 c_x->size[1] = b_A->size[1];
10597 cartes_emxEnsureCapacity_real_T(c_x, localB->kend, localB);
10598 localB->ix_i = b_A->size[0] * b_A->size[1] - 1;
10599 for (localB->kend = 0; localB->kend <= localB->ix_i; localB->kend++) {
10600 c_x->data[localB->kend] = b_A->data[localB->kend];
10601 }
10602
10603 localB->smax = b_A->data[localB->ii];
10604 tau->data[localB->m_gr] = 0.0;
10605 if (localB->mmi + 1 > 0) {
10606 localB->scale_c = cartesian_trajectory_p_xnrm2_ad(localB->mmi, b_A,
10607 localB->ii + 2, localB);
10608 if (localB->scale_c != 0.0) {
10609 localB->scale_c = rt_hypotd_snf(b_A->data[localB->ii], localB->scale_c);
10610 if (b_A->data[localB->ii] >= 0.0) {
10611 localB->scale_c = -localB->scale_c;
10612 }
10613
10614 if (fabs(localB->scale_c) < 1.0020841800044864E-292) {
10615 localB->kend = -1;
10616 localB->ix_i = (localB->ii + localB->mmi) + 1;
10617 do {
10618 localB->kend++;
10619 for (localB->itemp = localB->pvt; localB->itemp <= localB->ix_i;
10620 localB->itemp++) {
10621 c_x->data[localB->itemp - 1] *= 9.9792015476736E+291;
10622 }
10623
10624 localB->scale_c *= 9.9792015476736E+291;
10625 localB->smax *= 9.9792015476736E+291;
10626 } while (!(fabs(localB->scale_c) >= 1.0020841800044864E-292));
10627
10628 localB->scale_c = rt_hypotd_snf(localB->smax,
10629 cartesian_trajectory_p_xnrm2_ad(localB->mmi, c_x, localB->ii + 2,
10630 localB));
10631 if (localB->smax >= 0.0) {
10632 localB->scale_c = -localB->scale_c;
10633 }
10634
10635 tau->data[localB->m_gr] = (localB->scale_c - localB->smax) /
10636 localB->scale_c;
10637 localB->smax = 1.0 / (localB->smax - localB->scale_c);
10638 for (localB->itemp = localB->pvt; localB->itemp <= localB->ix_i;
10639 localB->itemp++) {
10640 c_x->data[localB->itemp - 1] *= localB->smax;
10641 }
10642
10643 for (localB->itemp = 0; localB->itemp <= localB->kend; localB->itemp
10644 ++) {
10645 localB->scale_c *= 1.0020841800044864E-292;
10646 }
10647
10648 localB->smax = localB->scale_c;
10649 } else {
10650 tau->data[localB->m_gr] = (localB->scale_c - b_A->data[localB->ii]) /
10651 localB->scale_c;
10652 localB->smax = 1.0 / (b_A->data[localB->ii] - localB->scale_c);
10653 localB->kend = c_x->size[0] * c_x->size[1];
10654 c_x->size[0] = b_A->size[0];
10655 c_x->size[1] = b_A->size[1];
10656 cartes_emxEnsureCapacity_real_T(c_x, localB->kend, localB);
10657 localB->ix_i = b_A->size[0] * b_A->size[1] - 1;
10658 for (localB->kend = 0; localB->kend <= localB->ix_i; localB->kend++)
10659 {
10660 c_x->data[localB->kend] = b_A->data[localB->kend];
10661 }
10662
10663 localB->b_g = (localB->ii + localB->mmi) + 1;
10664 for (localB->itemp = localB->pvt; localB->itemp <= localB->b_g;
10665 localB->itemp++) {
10666 c_x->data[localB->itemp - 1] *= localB->smax;
10667 }
10668
10669 localB->smax = localB->scale_c;
10670 }
10671 }
10672 }
10673
10674 localB->kend = b_A->size[0] * b_A->size[1];
10675 b_A->size[0] = c_x->size[0];
10676 b_A->size[1] = c_x->size[1];
10677 cartes_emxEnsureCapacity_real_T(b_A, localB->kend, localB);
10678 localB->ix_i = c_x->size[0] * c_x->size[1] - 1;
10679 for (localB->kend = 0; localB->kend <= localB->ix_i; localB->kend++) {
10680 b_A->data[localB->kend] = c_x->data[localB->kend];
10681 }
10682
10683 b_A->data[localB->ii] = localB->smax;
10684 } else {
10685 tau->data[localB->m_gr] = 0.0;
10686 }
10687
10688 if (localB->m_gr + 1 < n) {
10689 localB->smax = b_A->data[localB->ii];
10690 b_A->data[localB->ii] = 1.0;
10691 localB->pvt = (localB->ii + localB->ma) + 1;
10692 localB->kend = c_x->size[0] * c_x->size[1];
10693 c_x->size[0] = b_A->size[0];
10694 c_x->size[1] = b_A->size[1];
10695 cartes_emxEnsureCapacity_real_T(c_x, localB->kend, localB);
10696 localB->ix_i = b_A->size[0] * b_A->size[1] - 1;
10697 for (localB->kend = 0; localB->kend <= localB->ix_i; localB->kend++) {
10698 c_x->data[localB->kend] = b_A->data[localB->kend];
10699 }
10700
10701 if (tau->data[localB->m_gr] != 0.0) {
10702 localB->itemp = localB->mmi;
10703 localB->kend = localB->ii + localB->mmi;
10704 while ((localB->itemp + 1 > 0) && (b_A->data[localB->kend] == 0.0)) {
10705 localB->itemp--;
10706 localB->kend--;
10707 }
10708
10709 localB->nmi--;
10710 exitg2 = false;
10711 while ((!exitg2) && (localB->nmi > 0)) {
10712 localB->ix_i = (localB->nmi - 1) * localB->ma + localB->pvt;
10713 localB->kend = localB->ix_i;
10714 do {
10715 exitg1 = 0;
10716 if (localB->kend <= localB->ix_i + localB->itemp) {
10717 if (b_A->data[localB->kend - 1] != 0.0) {
10718 exitg1 = 1;
10719 } else {
10720 localB->kend++;
10721 }
10722 } else {
10723 localB->nmi--;
10724 exitg1 = 2;
10725 }
10726 } while (exitg1 == 0);
10727
10728 if (exitg1 == 1) {
10729 exitg2 = true;
10730 }
10731 }
10732
10733 localB->lastc = localB->nmi - 1;
10734 localB->kend = c_x->size[0] * c_x->size[1];
10735 c_x->size[0] = b_A->size[0];
10736 c_x->size[1] = b_A->size[1];
10737 cartes_emxEnsureCapacity_real_T(c_x, localB->kend, localB);
10738 localB->ix_i = b_A->size[0] * b_A->size[1] - 1;
10739 for (localB->kend = 0; localB->kend <= localB->ix_i; localB->kend++) {
10740 c_x->data[localB->kend] = b_A->data[localB->kend];
10741 }
10742 } else {
10743 localB->itemp = -1;
10744 localB->lastc = -1;
10745 }
10746
10747 if (localB->itemp + 1 > 0) {
10748 if (localB->lastc + 1 != 0) {
10749 for (localB->kend = 0; localB->kend <= localB->lastc; localB->kend++)
10750 {
10751 work->data[localB->kend] = 0.0;
10752 }
10753
10754 localB->iy_g = 0;
10755 localB->b_g = localB->ma * localB->lastc + localB->pvt;
10756 for (localB->nmi = localB->pvt; localB->ma < 0 ? localB->nmi >=
10757 localB->b_g : localB->nmi <= localB->b_g; localB->nmi +=
10758 localB->ma) {
10759 localB->ix_i = localB->ii;
10760 localB->scale_c = 0.0;
10761 localB->d_g = localB->nmi + localB->itemp;
10762 for (localB->kend = localB->nmi; localB->kend <= localB->d_g;
10763 localB->kend++) {
10764 localB->scale_c += c_x->data[localB->kend - 1] * c_x->data
10765 [localB->ix_i];
10766 localB->ix_i++;
10767 }
10768
10769 work->data[localB->iy_g] += localB->scale_c;
10770 localB->iy_g++;
10771 }
10772 }
10773
10774 if (!(-tau->data[localB->m_gr] == 0.0)) {
10775 localB->iy_g = 0;
10776 for (localB->kend = 0; localB->kend <= localB->lastc; localB->kend++)
10777 {
10778 if (work->data[localB->iy_g] != 0.0) {
10779 localB->scale_c = work->data[localB->iy_g] * -tau->data
10780 [localB->m_gr];
10781 localB->ix_i = localB->ii;
10782 localB->b_g = localB->itemp + localB->pvt;
10783 for (localB->nmi = localB->pvt; localB->nmi <= localB->b_g;
10784 localB->nmi++) {
10785 c_x->data[localB->nmi - 1] += c_x->data[localB->ix_i] *
10786 localB->scale_c;
10787 localB->ix_i++;
10788 }
10789 }
10790
10791 localB->iy_g++;
10792 localB->pvt += localB->ma;
10793 }
10794 }
10795 }
10796
10797 localB->kend = b_A->size[0] * b_A->size[1];
10798 b_A->size[0] = c_x->size[0];
10799 b_A->size[1] = c_x->size[1];
10800 cartes_emxEnsureCapacity_real_T(b_A, localB->kend, localB);
10801 localB->ix_i = c_x->size[0] * c_x->size[1] - 1;
10802 for (localB->kend = 0; localB->kend <= localB->ix_i; localB->kend++) {
10803 b_A->data[localB->kend] = c_x->data[localB->kend];
10804 }
10805
10806 b_A->data[localB->ii] = localB->smax;
10807 }
10808
10809 for (localB->ii = localB->m_gr + 2; localB->ii <= n; localB->ii++) {
10810 localB->pvt = ((localB->ii - 1) * localB->ma + localB->m_gr) + 1;
10811 if (vn1->data[localB->ii - 1] != 0.0) {
10812 localB->smax = fabs(b_A->data[localB->pvt - 1]) / vn1->data[localB->ii -
10813 1];
10814 localB->smax = 1.0 - localB->smax * localB->smax;
10815 if (localB->smax < 0.0) {
10816 localB->smax = 0.0;
10817 }
10818
10819 localB->scale_c = vn1->data[localB->ii - 1] / vn2->data[localB->ii - 1];
10820 localB->scale_c = localB->scale_c * localB->scale_c * localB->smax;
10821 if (localB->scale_c <= 1.4901161193847656E-8) {
10822 if (localB->m_gr + 1 < m) {
10823 localB->smax = 0.0;
10824 if (localB->mmi >= 1) {
10825 if (localB->mmi == 1) {
10826 localB->smax = fabs(b_A->data[localB->pvt]);
10827 } else {
10828 localB->scale_c = 3.3121686421112381E-170;
10829 localB->kend = localB->pvt + localB->mmi;
10830 for (localB->itemp = localB->pvt + 1; localB->itemp <=
10831 localB->kend; localB->itemp++) {
10832 localB->absxk = fabs(b_A->data[localB->itemp - 1]);
10833 if (localB->absxk > localB->scale_c) {
10834 localB->t = localB->scale_c / localB->absxk;
10835 localB->smax = localB->smax * localB->t * localB->t + 1.0;
10836 localB->scale_c = localB->absxk;
10837 } else {
10838 localB->t = localB->absxk / localB->scale_c;
10839 localB->smax += localB->t * localB->t;
10840 }
10841 }
10842
10843 localB->smax = localB->scale_c * sqrt(localB->smax);
10844 }
10845 }
10846
10847 vn1->data[localB->ii - 1] = localB->smax;
10848 vn2->data[localB->ii - 1] = vn1->data[localB->ii - 1];
10849 } else {
10850 vn1->data[localB->ii - 1] = 0.0;
10851 vn2->data[localB->ii - 1] = 0.0;
10852 }
10853 } else {
10854 vn1->data[localB->ii - 1] *= sqrt(localB->smax);
10855 }
10856 }
10857 }
10858 }
10859
10860 cartesian_trajec_emxFree_real_T(&c_x);
10861 cartesian_trajec_emxFree_real_T(&vn2);
10862 cartesian_trajec_emxFree_real_T(&vn1);
10863 cartesian_trajec_emxFree_real_T(&work);
10864}
10865
10866static void cartesian_trajectory_p_mldivide(const
10867 emxArray_real_T_cartesian_tra_T *A, const emxArray_real_T_cartesian_tra_T *B,
10868 emxArray_real_T_cartesian_tra_T *Y, B_MATLABSystem_cartesian_traj_T *localB)
10869{
10870 emxArray_real_T_cartesian_tra_T *c_A;
10871 emxArray_real_T_cartesian_tra_T *b_tau;
10872 emxArray_int32_T_cartesian_tr_T *b_jpvt;
10873 emxArray_real_T_cartesian_tra_T *B_0;
10874 emxArray_int32_T_cartesian_tr_T *b_jpvt_0;
10875 boolean_T guard1 = false;
10876 cartesian_trajec_emxInit_real_T(&c_A, 2, localB);
10877 cartesian_trajec_emxInit_real_T(&b_tau, 1, localB);
10878 cartesian_traje_emxInit_int32_T(&b_jpvt, 2);
10879 cartesian_trajec_emxInit_real_T(&B_0, 2, localB);
10880 cartesian_traje_emxInit_int32_T(&b_jpvt_0, 2);
10881 if ((A->size[0] == 0) || (A->size[1] == 0) || ((B->size[0] == 0) || (B->size[1]
10882 == 0))) {
10883 localB->minmn = A->size[1];
10884 localB->minmana = B->size[1];
10885 localB->b_i_gz = Y->size[0] * Y->size[1];
10886 Y->size[0] = localB->minmn;
10887 Y->size[1] = localB->minmana;
10888 cartes_emxEnsureCapacity_real_T(Y, localB->b_i_gz, localB);
10889 localB->minmn = localB->minmn * localB->minmana - 1;
10890 for (localB->b_i_gz = 0; localB->b_i_gz <= localB->minmn; localB->b_i_gz++)
10891 {
10892 Y->data[localB->b_i_gz] = 0.0;
10893 }
10894 } else if (A->size[0] == A->size[1]) {
10895 localB->minmn = A->size[0];
10896 localB->rankR = A->size[1];
10897 if (localB->minmn < localB->rankR) {
10898 localB->rankR = localB->minmn;
10899 }
10900
10901 localB->minmn = B->size[0];
10902 if (localB->minmn < localB->rankR) {
10903 localB->rankR = localB->minmn;
10904 }
10905
10906 localB->nb = B->size[1] - 1;
10907 cartesian_trajectory_pl_xzgetrf(localB->rankR, localB->rankR, A, A->size[0],
10908 c_A, b_jpvt, &localB->minmn, localB);
10909 localB->b_i_gz = B_0->size[0] * B_0->size[1];
10910 B_0->size[0] = B->size[0];
10911 B_0->size[1] = B->size[1];
10912 cartes_emxEnsureCapacity_real_T(B_0, localB->b_i_gz, localB);
10913 localB->minmn = B->size[0] * B->size[1] - 1;
10914 for (localB->b_i_gz = 0; localB->b_i_gz <= localB->minmn; localB->b_i_gz++)
10915 {
10916 B_0->data[localB->b_i_gz] = B->data[localB->b_i_gz];
10917 }
10918
10919 localB->minmn = localB->rankR - 2;
10920 for (localB->b_i_gz = 0; localB->b_i_gz <= localB->minmn; localB->b_i_gz++)
10921 {
10922 if (localB->b_i_gz + 1 != b_jpvt->data[localB->b_i_gz]) {
10923 localB->na = b_jpvt->data[localB->b_i_gz] - 1;
10924 for (localB->minmana = 0; localB->minmana <= localB->nb; localB->minmana
10925 ++) {
10926 localB->tol_l = B_0->data[B_0->size[0] * localB->minmana +
10927 localB->b_i_gz];
10928 B_0->data[localB->b_i_gz + B_0->size[0] * localB->minmana] = B_0->
10929 data[B_0->size[0] * localB->minmana + localB->na];
10930 B_0->data[localB->na + B_0->size[0] * localB->minmana] = localB->tol_l;
10931 }
10932 }
10933 }
10934
10935 if ((B->size[1] == 0) || ((B_0->size[0] == 0) || (B_0->size[1] == 0))) {
10936 } else {
10937 for (localB->minmana = 0; localB->minmana <= localB->nb; localB->minmana++)
10938 {
10939 localB->m_m = B->size[0] * localB->minmana - 1;
10940 for (localB->minmn = 0; localB->minmn < localB->rankR; localB->minmn++)
10941 {
10942 localB->nb_o = c_A->size[0] * localB->minmn - 1;
10943 localB->b_i_gz = (localB->minmn + localB->m_m) + 1;
10944 if (B_0->data[localB->b_i_gz] != 0.0) {
10945 for (localB->na = localB->minmn + 2; localB->na <= localB->rankR;
10946 localB->na++) {
10947 localB->mn = localB->na + localB->m_m;
10948 B_0->data[localB->mn] -= B_0->data[localB->b_i_gz] * c_A->
10949 data[localB->na + localB->nb_o];
10950 }
10951 }
10952 }
10953 }
10954 }
10955
10956 cartesian_trajectory_plan_xtrsm(localB->rankR, B->size[1], c_A, c_A->size[0],
10957 B_0, B->size[0], Y, localB);
10958 } else {
10959 localB->na = A->size[1] - 1;
10960 localB->b_i_gz = c_A->size[0] * c_A->size[1];
10961 c_A->size[0] = A->size[0];
10962 c_A->size[1] = A->size[1];
10963 cartes_emxEnsureCapacity_real_T(c_A, localB->b_i_gz, localB);
10964 localB->minmn = A->size[0] * A->size[1] - 1;
10965 for (localB->b_i_gz = 0; localB->b_i_gz <= localB->minmn; localB->b_i_gz++)
10966 {
10967 c_A->data[localB->b_i_gz] = A->data[localB->b_i_gz];
10968 }
10969
10970 localB->minmn = A->size[0];
10971 localB->minmana = A->size[1];
10972 if (localB->minmn < localB->minmana) {
10973 localB->minmana = localB->minmn;
10974 }
10975
10976 localB->b_i_gz = b_tau->size[0];
10977 b_tau->size[0] = localB->minmana;
10978 cartes_emxEnsureCapacity_real_T(b_tau, localB->b_i_gz, localB);
10979 for (localB->b_i_gz = 0; localB->b_i_gz < localB->minmana; localB->b_i_gz++)
10980 {
10981 b_tau->data[localB->b_i_gz] = 0.0;
10982 }
10983
10984 guard1 = false;
10985 if ((A->size[0] == 0) || (A->size[1] == 0)) {
10986 guard1 = true;
10987 } else {
10988 localB->minmn = A->size[0];
10989 localB->minmana = A->size[1];
10990 if (localB->minmn < localB->minmana) {
10991 localB->minmana = localB->minmn;
10992 }
10993
10994 if (localB->minmana < 1) {
10995 guard1 = true;
10996 } else {
10997 localB->b_i_gz = b_jpvt->size[0] * b_jpvt->size[1];
10998 b_jpvt->size[0] = 1;
10999 b_jpvt->size[1] = A->size[1];
11000 carte_emxEnsureCapacity_int32_T(b_jpvt, localB->b_i_gz);
11001 localB->minmn = A->size[1] - 1;
11002 for (localB->b_i_gz = 0; localB->b_i_gz <= localB->minmn; localB->b_i_gz
11003 ++) {
11004 b_jpvt->data[localB->b_i_gz] = 0;
11005 }
11006
11007 for (localB->minmn = 0; localB->minmn <= localB->na; localB->minmn++) {
11008 b_jpvt->data[localB->minmn] = localB->minmn + 1;
11009 }
11010
11011 localB->b_i_gz = b_jpvt_0->size[0] * b_jpvt_0->size[1];
11012 b_jpvt_0->size[0] = 1;
11013 b_jpvt_0->size[1] = b_jpvt->size[1];
11014 carte_emxEnsureCapacity_int32_T(b_jpvt_0, localB->b_i_gz);
11015 localB->minmn = b_jpvt->size[0] * b_jpvt->size[1];
11016 for (localB->b_i_gz = 0; localB->b_i_gz < localB->minmn; localB->b_i_gz
11017 ++) {
11018 b_jpvt_0->data[localB->b_i_gz] = b_jpvt->data[localB->b_i_gz];
11019 }
11020
11021 cartesian_trajectory_pla_qrpf_b(A, A->size[0], A->size[1], b_tau,
11022 b_jpvt_0, c_A, b_jpvt, localB);
11023 }
11024 }
11025
11026 if (guard1) {
11027 localB->b_i_gz = b_jpvt->size[0] * b_jpvt->size[1];
11028 b_jpvt->size[0] = 1;
11029 b_jpvt->size[1] = A->size[1];
11030 carte_emxEnsureCapacity_int32_T(b_jpvt, localB->b_i_gz);
11031 localB->minmn = A->size[1] - 1;
11032 for (localB->b_i_gz = 0; localB->b_i_gz <= localB->minmn; localB->b_i_gz++)
11033 {
11034 b_jpvt->data[localB->b_i_gz] = 0;
11035 }
11036
11037 for (localB->minmana = 0; localB->minmana <= localB->na; localB->minmana++)
11038 {
11039 b_jpvt->data[localB->minmana] = localB->minmana + 1;
11040 }
11041 }
11042
11043 localB->rankR = 0;
11044 if (c_A->size[0] < c_A->size[1]) {
11045 localB->minmn = c_A->size[0];
11046 localB->minmana = c_A->size[1];
11047 } else {
11048 localB->minmn = c_A->size[1];
11049 localB->minmana = c_A->size[0];
11050 }
11051
11052 if (localB->minmn > 0) {
11053 localB->tol_l = 2.2204460492503131E-15 * static_cast<real_T>
11054 (localB->minmana);
11055 if (1.4901161193847656E-8 < localB->tol_l) {
11056 localB->tol_l = 1.4901161193847656E-8;
11057 }
11058
11059 localB->tol_l *= fabs(c_A->data[0]);
11060 while ((localB->rankR < localB->minmn) && (!(fabs(c_A->data[c_A->size[0] *
11061 localB->rankR + localB->rankR]) <= localB->tol_l))) {
11062 localB->rankR++;
11063 }
11064 }
11065
11066 localB->nb = B->size[1] - 1;
11067 localB->minmn = c_A->size[1];
11068 localB->minmana = B->size[1];
11069 localB->b_i_gz = Y->size[0] * Y->size[1];
11070 Y->size[0] = localB->minmn;
11071 Y->size[1] = localB->minmana;
11072 cartes_emxEnsureCapacity_real_T(Y, localB->b_i_gz, localB);
11073 localB->minmn = localB->minmn * localB->minmana - 1;
11074 for (localB->b_i_gz = 0; localB->b_i_gz <= localB->minmn; localB->b_i_gz++)
11075 {
11076 Y->data[localB->b_i_gz] = 0.0;
11077 }
11078
11079 localB->b_i_gz = B_0->size[0] * B_0->size[1];
11080 B_0->size[0] = B->size[0];
11081 B_0->size[1] = B->size[1];
11082 cartes_emxEnsureCapacity_real_T(B_0, localB->b_i_gz, localB);
11083 localB->minmn = B->size[0] * B->size[1] - 1;
11084 for (localB->b_i_gz = 0; localB->b_i_gz <= localB->minmn; localB->b_i_gz++)
11085 {
11086 B_0->data[localB->b_i_gz] = B->data[localB->b_i_gz];
11087 }
11088
11089 localB->m_m = c_A->size[0];
11090 localB->nb_o = B->size[1] - 1;
11091 localB->minmn = c_A->size[0];
11092 localB->minmana = c_A->size[1];
11093 if (localB->minmn < localB->minmana) {
11094 localB->minmana = localB->minmn;
11095 }
11096
11097 localB->mn = localB->minmana - 1;
11098 for (localB->minmana = 0; localB->minmana <= localB->mn; localB->minmana++)
11099 {
11100 if (b_tau->data[localB->minmana] != 0.0) {
11101 for (localB->minmn = 0; localB->minmn <= localB->nb_o; localB->minmn++)
11102 {
11103 localB->tol_l = B_0->data[B_0->size[0] * localB->minmn +
11104 localB->minmana];
11105 for (localB->na = localB->minmana + 2; localB->na <= localB->m_m;
11106 localB->na++) {
11107 localB->tol_l += c_A->data[(c_A->size[0] * localB->minmana +
11108 localB->na) - 1] * B_0->data[(B_0->size[0] * localB->minmn +
11109 localB->na) - 1];
11110 }
11111
11112 localB->tol_l *= b_tau->data[localB->minmana];
11113 if (localB->tol_l != 0.0) {
11114 B_0->data[localB->minmana + B_0->size[0] * localB->minmn] -=
11115 localB->tol_l;
11116 for (localB->b_i_gz = localB->minmana + 2; localB->b_i_gz <=
11117 localB->m_m; localB->b_i_gz++) {
11118 B_0->data[(localB->b_i_gz + B_0->size[0] * localB->minmn) - 1] -=
11119 c_A->data[(c_A->size[0] * localB->minmana + localB->b_i_gz) - 1]
11120 * localB->tol_l;
11121 }
11122 }
11123 }
11124 }
11125 }
11126
11127 for (localB->minmn = 0; localB->minmn <= localB->nb; localB->minmn++) {
11128 for (localB->b_i_gz = 0; localB->b_i_gz < localB->rankR; localB->b_i_gz++)
11129 {
11130 Y->data[(b_jpvt->data[localB->b_i_gz] + Y->size[0] * localB->minmn) - 1]
11131 = B_0->data[B_0->size[0] * localB->minmn + localB->b_i_gz];
11132 }
11133
11134 for (localB->minmana = localB->rankR; localB->minmana >= 1;
11135 localB->minmana--) {
11136 Y->data[(b_jpvt->data[localB->minmana - 1] + Y->size[0] * localB->minmn)
11137 - 1] /= c_A->data[((localB->minmana - 1) * c_A->size[0] +
11138 localB->minmana) - 1];
11139 localB->na = localB->minmana - 2;
11140 for (localB->b_i_gz = 0; localB->b_i_gz <= localB->na; localB->b_i_gz++)
11141 {
11142 Y->data[(b_jpvt->data[localB->b_i_gz] + Y->size[0] * localB->minmn) -
11143 1] -= Y->data[(b_jpvt->data[localB->minmana - 1] + Y->size[0] *
11144 localB->minmn) - 1] * c_A->data[(localB->minmana - 1)
11145 * c_A->size[0] + localB->b_i_gz];
11146 }
11147 }
11148 }
11149 }
11150
11151 cartesian_traje_emxFree_int32_T(&b_jpvt_0);
11152 cartesian_trajec_emxFree_real_T(&B_0);
11153 cartesian_traje_emxFree_int32_T(&b_jpvt);
11154 cartesian_trajec_emxFree_real_T(&b_tau);
11155 cartesian_trajec_emxFree_real_T(&c_A);
11156}
11157
11158static void cartesian_trajectory_planne_inv(const
11159 emxArray_real_T_cartesian_tra_T *x, emxArray_real_T_cartesian_tra_T *y,
11160 B_MATLABSystem_cartesian_traj_T *localB)
11161{
11162 int32_T n;
11163 emxArray_int32_T_cartesian_tr_T *p;
11164 int32_T c;
11165 emxArray_real_T_cartesian_tra_T *c_A;
11166 emxArray_int32_T_cartesian_tr_T *b_ipiv;
11167 int32_T info;
11168 int32_T n_0;
11169 int32_T yk;
11170 emxArray_real_T_cartesian_tra_T *y_0;
11171 if ((x->size[0] == 0) || (x->size[1] == 0)) {
11172 info = y->size[0] * y->size[1];
11173 y->size[0] = x->size[0];
11174 y->size[1] = x->size[1];
11175 cartes_emxEnsureCapacity_real_T(y, info, localB);
11176 n_0 = x->size[0] * x->size[1] - 1;
11177 for (info = 0; info <= n_0; info++) {
11178 y->data[info] = x->data[info];
11179 }
11180 } else {
11181 n = x->size[0];
11182 info = y->size[0] * y->size[1];
11183 y->size[0] = x->size[0];
11184 y->size[1] = x->size[1];
11185 cartes_emxEnsureCapacity_real_T(y, info, localB);
11186 n_0 = x->size[0] * x->size[1] - 1;
11187 for (info = 0; info <= n_0; info++) {
11188 y->data[info] = 0.0;
11189 }
11190
11191 cartesian_traje_emxInit_int32_T(&p, 2);
11192 cartesian_trajec_emxInit_real_T(&c_A, 2, localB);
11193 cartesian_traje_emxInit_int32_T(&b_ipiv, 2);
11194 cartesian_trajectory_pl_xzgetrf(x->size[0], x->size[0], x, x->size[0], c_A,
11195 b_ipiv, &info, localB);
11196 if (x->size[0] < 1) {
11197 n_0 = 0;
11198 } else {
11199 n_0 = x->size[0];
11200 }
11201
11202 info = p->size[0] * p->size[1];
11203 p->size[0] = 1;
11204 p->size[1] = n_0;
11205 carte_emxEnsureCapacity_int32_T(p, info);
11206 if (n_0 > 0) {
11207 p->data[0] = 1;
11208 yk = 1;
11209 for (info = 2; info <= n_0; info++) {
11210 yk++;
11211 p->data[info - 1] = yk;
11212 }
11213 }
11214
11215 n_0 = b_ipiv->size[1] - 1;
11216 for (info = 0; info <= n_0; info++) {
11217 if (b_ipiv->data[info] > static_cast<real_T>(info) + 1.0) {
11218 yk = p->data[b_ipiv->data[info] - 1];
11219 p->data[b_ipiv->data[info] - 1] = p->data[info];
11220 p->data[info] = yk;
11221 }
11222 }
11223
11224 cartesian_traje_emxFree_int32_T(&b_ipiv);
11225 for (info = 0; info < n; info++) {
11226 c = p->data[info] - 1;
11227 y->data[info + y->size[0] * (p->data[info] - 1)] = 1.0;
11228 for (n_0 = info + 1; n_0 <= n; n_0++) {
11229 if (y->data[(y->size[0] * c + n_0) - 1] != 0.0) {
11230 for (yk = n_0 + 1; yk <= n; yk++) {
11231 y->data[(yk + y->size[0] * c) - 1] -= c_A->data[((n_0 - 1) *
11232 c_A->size[0] + yk) - 1] * y->data[(y->size[0] * c + n_0) - 1];
11233 }
11234 }
11235 }
11236 }
11237
11238 cartesian_traje_emxFree_int32_T(&p);
11239 cartesian_trajec_emxInit_real_T(&y_0, 2, localB);
11240 info = y_0->size[0] * y_0->size[1];
11241 y_0->size[0] = y->size[0];
11242 y_0->size[1] = y->size[1];
11243 cartes_emxEnsureCapacity_real_T(y_0, info, localB);
11244 n_0 = y->size[0] * y->size[1];
11245 for (info = 0; info < n_0; info++) {
11246 y_0->data[info] = y->data[info];
11247 }
11248
11249 cartesian_trajectory_plan_xtrsm(x->size[0], x->size[0], c_A, x->size[0], y_0,
11250 x->size[0], y, localB);
11251 cartesian_trajec_emxFree_real_T(&y_0);
11252 cartesian_trajec_emxFree_real_T(&c_A);
11253 }
11254}
11255
11256static void cartesian_trajectory_plann_diag(const
11257 emxArray_real_T_cartesian_tra_T *v, emxArray_real_T_cartesian_tra_T *d,
11258 B_MATLABSystem_cartesian_traj_T *localB)
11259{
11260 int32_T u0;
11261 int32_T u1;
11262 if ((v->size[0] == 1) && (v->size[1] == 1)) {
11263 u0 = d->size[0];
11264 d->size[0] = 1;
11265 cartes_emxEnsureCapacity_real_T(d, u0, localB);
11266 d->data[0] = v->data[0];
11267 } else {
11268 if (0 < v->size[1]) {
11269 u0 = v->size[0];
11270 u1 = v->size[1];
11271 if (u0 < u1) {
11272 u1 = u0;
11273 }
11274 } else {
11275 u1 = 0;
11276 }
11277
11278 u0 = d->size[0];
11279 d->size[0] = u1;
11280 cartes_emxEnsureCapacity_real_T(d, u0, localB);
11281 for (u0 = 0; u0 < u1; u0++) {
11282 d->data[u0] = v->data[v->size[0] * u0 + u0];
11283 }
11284 }
11285}
11286
11287static boolean_T cartesian_trajectory_planne_any(const
11288 emxArray_boolean_T_cartesian__T *x)
11289{
11290 boolean_T y;
11291 int32_T ix;
11292 boolean_T exitg1;
11293 y = false;
11294 ix = 0;
11295 exitg1 = false;
11296 while ((!exitg1) && (ix + 1 <= x->size[0])) {
11297 if (!x->data[ix]) {
11298 ix++;
11299 } else {
11300 y = true;
11301 exitg1 = true;
11302 }
11303 }
11304
11305 return y;
11306}
11307
11308static void cartesian_trajectory_pl_sqrt_nh(emxArray_real_T_cartesian_tra_T *x)
11309{
11310 int32_T nx;
11311 int32_T b_k;
11312 nx = x->size[0] - 1;
11313 for (b_k = 0; b_k <= nx; b_k++) {
11314 x->data[b_k] = sqrt(x->data[b_k]);
11315 }
11316}
11317
11318static boolean_T cartesian_tr_isPositiveDefinite(const real_T B[36],
11319 B_MATLABSystem_cartesian_traj_T *localB)
11320{
11321 boolean_T flag;
11322 emxArray_real_T_cartesian_tra_T *b_x;
11323 boolean_T exitg1;
11324 localB->c_A_size_idx_0 = 6;
11325 localB->c_A_size_idx_1 = 6;
11326 memcpy(&localB->c_A_data[0], &B[0], 36U * sizeof(real_T));
11327 localB->b_info = 0;
11328 localB->b_j_e = 1;
11329 cartesian_trajec_emxInit_real_T(&b_x, 2, localB);
11330 exitg1 = false;
11331 while ((!exitg1) && (localB->b_j_e - 1 < 6)) {
11332 localB->jm1 = localB->b_j_e - 2;
11333 localB->idxAjj = ((localB->b_j_e - 1) * 6 + localB->b_j_e) - 1;
11334 localB->ssq = 0.0;
11335 if (localB->b_j_e - 1 >= 1) {
11336 localB->ix_o = localB->b_j_e - 1;
11337 localB->iy = localB->b_j_e - 1;
11338 for (localB->k_fr = 0; localB->k_fr <= localB->jm1; localB->k_fr++) {
11339 localB->ssq += localB->c_A_data[localB->ix_o] * localB->c_A_data
11340 [localB->iy];
11341 localB->ix_o += 6;
11342 localB->iy += 6;
11343 }
11344 }
11345
11346 localB->ssq = localB->c_A_data[localB->idxAjj] - localB->ssq;
11347 if (localB->ssq > 0.0) {
11348 localB->ssq = sqrt(localB->ssq);
11349 localB->c_A_data[localB->idxAjj] = localB->ssq;
11350 if (localB->b_j_e < 6) {
11351 if (localB->b_j_e - 1 != 0) {
11352 localB->ix_o = localB->b_j_e - 1;
11353 localB->jm1 = (localB->b_j_e - 2) * 6 + localB->b_j_e;
11354 for (localB->k_fr = localB->b_j_e + 1; localB->k_fr <= localB->jm1 + 1;
11355 localB->k_fr += 6) {
11356 localB->c = -localB->c_A_data[localB->ix_o];
11357 localB->iy = localB->idxAjj + 1;
11358 localB->d_j = localB->k_fr - localB->b_j_e;
11359 for (localB->ia = localB->k_fr; localB->ia <= localB->d_j + 5;
11360 localB->ia++) {
11361 localB->c_A_data[localB->iy] += localB->c_A_data[localB->ia - 1] *
11362 localB->c;
11363 localB->iy++;
11364 }
11365
11366 localB->ix_o += 6;
11367 }
11368 }
11369
11370 localB->ssq = 1.0 / localB->ssq;
11371 localB->ix_o = b_x->size[0] * b_x->size[1];
11372 b_x->size[0] = 6;
11373 b_x->size[1] = 6;
11374 cartes_emxEnsureCapacity_real_T(b_x, localB->ix_o, localB);
11375 localB->c_A_size_idx_0 = localB->c_A_size_idx_0 * localB->c_A_size_idx_1
11376 - 1;
11377 for (localB->ix_o = 0; localB->ix_o <= localB->c_A_size_idx_0;
11378 localB->ix_o++) {
11379 b_x->data[localB->ix_o] = localB->c_A_data[localB->ix_o];
11380 }
11381
11382 localB->jm1 = localB->idxAjj - localB->b_j_e;
11383 for (localB->k_fr = localB->idxAjj + 2; localB->k_fr <= localB->jm1 + 7;
11384 localB->k_fr++) {
11385 b_x->data[localB->k_fr - 1] *= localB->ssq;
11386 }
11387
11388 localB->c_A_size_idx_0 = b_x->size[0];
11389 localB->c_A_size_idx_1 = b_x->size[1];
11390 for (localB->ix_o = 0; localB->ix_o < 36; localB->ix_o++) {
11391 localB->c_A_data[localB->ix_o] = b_x->data[localB->ix_o];
11392 }
11393 }
11394
11395 localB->b_j_e++;
11396 } else {
11397 localB->b_info = localB->b_j_e;
11398 exitg1 = true;
11399 }
11400 }
11401
11402 cartesian_trajec_emxFree_real_T(&b_x);
11403 flag = (localB->b_info == 0);
11404 return flag;
11405}
11406
11407static boolean_T DampedBFGSwGradientProjection_k(const
11408 h_robotics_core_internal_Damp_T *obj, const real_T xNew[6],
11409 B_MATLABSystem_cartesian_traj_T *localB)
11410{
11411 boolean_T flag;
11412 emxArray_real_T_cartesian_tra_T *b;
11413 emxArray_real_T_cartesian_tra_T *c;
11414 int32_T m;
11415 int32_T inner;
11416 int32_T b_i;
11417 int32_T loop_ub;
11418 emxArray_boolean_T_cartesian__T *c_0;
11419 cartesian_trajec_emxInit_real_T(&b, 2, localB);
11420 cartesian_trajec_emxInit_real_T(&c, 1, localB);
11421 cartesian_tra_emxInit_boolean_T(&c_0, 1);
11422 if (obj->ConstraintsOn) {
11423 b_i = b->size[0] * b->size[1];
11424 b->size[0] = obj->ConstraintMatrix->size[0];
11425 b->size[1] = obj->ConstraintMatrix->size[1];
11426 cartes_emxEnsureCapacity_real_T(b, b_i, localB);
11427 loop_ub = obj->ConstraintMatrix->size[0] * obj->ConstraintMatrix->size[1] -
11428 1;
11429 for (b_i = 0; b_i <= loop_ub; b_i++) {
11430 b->data[b_i] = obj->ConstraintMatrix->data[b_i];
11431 }
11432
11433 m = b->size[1] - 1;
11434 inner = b->size[0] - 1;
11435 b_i = c->size[0];
11436 c->size[0] = b->size[1];
11437 cartes_emxEnsureCapacity_real_T(c, b_i, localB);
11438 for (b_i = 0; b_i <= m; b_i++) {
11439 c->data[b_i] = 0.0;
11440 }
11441
11442 for (b_i = 0; b_i <= inner; b_i++) {
11443 for (loop_ub = 0; loop_ub <= m; loop_ub++) {
11444 c->data[loop_ub] += b->data[loop_ub * b->size[0] + b_i] * xNew[b_i];
11445 }
11446 }
11447
11448 b_i = c_0->size[0];
11449 c_0->size[0] = c->size[0];
11450 car_emxEnsureCapacity_boolean_T(c_0, b_i);
11451 loop_ub = c->size[0];
11452 for (b_i = 0; b_i < loop_ub; b_i++) {
11453 c_0->data[b_i] = (c->data[b_i] - obj->ConstraintBound->data[b_i] >
11454 1.4901161193847656E-8);
11455 }
11456
11457 if (cartesian_trajectory_planne_any(c_0)) {
11458 flag = true;
11459 } else {
11460 flag = false;
11461 }
11462 } else {
11463 flag = false;
11464 }
11465
11466 cartesian_tra_emxFree_boolean_T(&c_0);
11467 cartesian_trajec_emxFree_real_T(&c);
11468 cartesian_trajec_emxFree_real_T(&b);
11469 return flag;
11470}
11471
11472static void DampedBFGSwGradientProjection_s(h_robotics_core_internal_Damp_T *obj,
11473 real_T xSol[6], c_robotics_core_internal_NLPS_T *exitFlag, real_T *err, real_T
11474 *iter, B_MATLABSystem_cartesian_traj_T *localB)
11475{
11476 emxArray_real_T_cartesian_tra_T *unusedU1;
11477 emxArray_real_T_cartesian_tra_T *grad;
11478 emxArray_boolean_T_cartesian__T *activeSet;
11479 emxArray_real_T_cartesian_tra_T *A;
11480 emxArray_real_T_cartesian_tra_T *alpha;
11481 emxArray_real_T_cartesian_tra_T *AIn;
11482 emxArray_real_T_cartesian_tra_T *L;
11483 f_robotics_manip_internal_IKE_T *b;
11484 f_robotics_manip_internal_IKE_T *c;
11485 f_robotics_manip_internal_IKE_T *d;
11486 emxArray_int32_T_cartesian_tr_T *cb;
11487 emxArray_int32_T_cartesian_tr_T *db;
11488 emxArray_int32_T_cartesian_tr_T *eb;
11489 emxArray_int32_T_cartesian_tr_T *fb;
11490 emxArray_int32_T_cartesian_tr_T *gb;
11491 f_robotics_manip_internal_IKE_T *args;
11492 emxArray_real_T_cartesian_tra_T *a;
11493 emxArray_int32_T_cartesian_tr_T *ii;
11494 emxArray_real_T_cartesian_tra_T *y;
11495 emxArray_int32_T_cartesian_tr_T *ii_0;
11496 emxArray_real_T_cartesian_tra_T *y_0;
11497 emxArray_boolean_T_cartesian__T *x;
11498 emxArray_real_T_cartesian_tra_T *A_0;
11499 emxArray_real_T_cartesian_tra_T *A_1;
11500 emxArray_real_T_cartesian_tra_T *A_2;
11501 emxArray_real_T_cartesian_tra_T *sigma;
11502 emxArray_real_T_cartesian_tra_T *tmp;
11503 emxArray_real_T_cartesian_tra_T *tmp_0;
11504 emxArray_real_T_cartesian_tra_T *grad_0;
11505 emxArray_real_T_cartesian_tra_T *sNew;
11506 emxArray_int32_T_cartesian_tr_T *ii_1;
11507 emxArray_int32_T_cartesian_tr_T *ii_2;
11508 emxArray_real_T_cartesian_tra_T *grad_1;
11509 emxArray_real_T_cartesian_tra_T *A_3;
11510 emxArray_real_T_cartesian_tra_T *alpha_0;
11511 emxArray_int32_T_cartesian_tr_T *ii_3;
11512 static const int8_T tmp_1[36] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1,
11513 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 };
11514
11515 int32_T exitg1;
11516 int32_T exitg2;
11517 boolean_T exitg3;
11518 boolean_T guard1 = false;
11519 boolean_T guard2 = false;
11520 for (localB->i_i = 0; localB->i_i < 6; localB->i_i++) {
11521 localB->x[localB->i_i] = obj->SeedInternal[localB->i_i];
11522 }
11523
11524 cartesian_trajec_emxInit_real_T(&unusedU1, 2, localB);
11525 cartesian_trajec_emxInit_real_T(&grad, 1, localB);
11526 obj->TimeObjInternal.StartTime = ctimefun();
11527 cartesian_IKHelpers_computeCost(localB->x, obj->ExtraArgs, &localB->cost,
11528 localB->unusedU0, unusedU1, &b, localB);
11529 obj->ExtraArgs = b;
11530 args = obj->ExtraArgs;
11531 localB->b_i_l = grad->size[0];
11532 grad->size[0] = args->GradTemp->size[0];
11533 cartes_emxEnsureCapacity_real_T(grad, localB->b_i_l, localB);
11534 localB->i_i = args->GradTemp->size[0];
11535 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l++) {
11536 grad->data[localB->b_i_l] = args->GradTemp->data[localB->b_i_l];
11537 }
11538
11539 cartesian_trajectory_planne_eye(localB->unusedU0);
11540 memcpy(&localB->H[0], &localB->unusedU0[0], 36U * sizeof(real_T));
11541 cartesian_tra_emxInit_boolean_T(&activeSet, 1);
11542 cartesian_trajec_emxInit_real_T(&A, 2, localB);
11543 cartesian_trajec_emxInit_real_T(&alpha, 1, localB);
11544 cartesian_traje_emxInit_int32_T(&ii, 1);
11545 if (obj->ConstraintsOn) {
11546 localB->b_i_l = A->size[0] * A->size[1];
11547 A->size[0] = obj->ConstraintMatrix->size[0];
11548 A->size[1] = obj->ConstraintMatrix->size[1];
11549 cartes_emxEnsureCapacity_real_T(A, localB->b_i_l, localB);
11550 localB->i_i = obj->ConstraintMatrix->size[0] * obj->ConstraintMatrix->size[1]
11551 - 1;
11552 for (localB->b_i_l = 0; localB->b_i_l <= localB->i_i; localB->b_i_l++) {
11553 A->data[localB->b_i_l] = obj->ConstraintMatrix->data[localB->b_i_l];
11554 }
11555
11556 localB->m_i = A->size[1] - 1;
11557 localB->inner = A->size[0] - 1;
11558 localB->b_i_l = alpha->size[0];
11559 alpha->size[0] = A->size[1];
11560 cartes_emxEnsureCapacity_real_T(alpha, localB->b_i_l, localB);
11561 for (localB->b_i_l = 0; localB->b_i_l <= localB->m_i; localB->b_i_l++) {
11562 alpha->data[localB->b_i_l] = 0.0;
11563 }
11564
11565 for (localB->nx_n = 0; localB->nx_n <= localB->inner; localB->nx_n++) {
11566 for (localB->g_idx_0 = 0; localB->g_idx_0 <= localB->m_i; localB->g_idx_0
11567 ++) {
11568 alpha->data[localB->g_idx_0] += A->data[localB->g_idx_0 * A->size[0] +
11569 localB->nx_n] * localB->x[localB->nx_n];
11570 }
11571 }
11572
11573 localB->b_i_l = activeSet->size[0];
11574 activeSet->size[0] = alpha->size[0];
11575 car_emxEnsureCapacity_boolean_T(activeSet, localB->b_i_l);
11576 localB->i_i = alpha->size[0];
11577 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l++) {
11578 activeSet->data[localB->b_i_l] = (alpha->data[localB->b_i_l] >=
11579 obj->ConstraintBound->data[localB->b_i_l]);
11580 }
11581
11582 localB->nx_n = activeSet->size[0] - 1;
11583 localB->idx = 0;
11584 for (localB->g_idx_0 = 0; localB->g_idx_0 <= localB->nx_n; localB->g_idx_0++)
11585 {
11586 if (activeSet->data[localB->g_idx_0]) {
11587 localB->idx++;
11588 }
11589 }
11590
11591 localB->b_i_l = ii->size[0];
11592 ii->size[0] = localB->idx;
11593 carte_emxEnsureCapacity_int32_T(ii, localB->b_i_l);
11594 localB->b_i_l = 0;
11595 for (localB->g_idx_0 = 0; localB->g_idx_0 <= localB->nx_n; localB->g_idx_0++)
11596 {
11597 if (activeSet->data[localB->g_idx_0]) {
11598 ii->data[localB->b_i_l] = localB->g_idx_0 + 1;
11599 localB->b_i_l++;
11600 }
11601 }
11602
11603 localB->i_i = obj->ConstraintMatrix->size[0];
11604 localB->b_i_l = A->size[0] * A->size[1];
11605 A->size[0] = localB->i_i;
11606 A->size[1] = ii->size[0];
11607 cartes_emxEnsureCapacity_real_T(A, localB->b_i_l, localB);
11608 localB->n = ii->size[0];
11609 for (localB->b_i_l = 0; localB->b_i_l < localB->n; localB->b_i_l++) {
11610 for (localB->idx = 0; localB->idx < localB->i_i; localB->idx++) {
11611 A->data[localB->idx + A->size[0] * localB->b_i_l] =
11612 obj->ConstraintMatrix->data[(ii->data[localB->b_i_l] - 1) *
11613 obj->ConstraintMatrix->size[0] + localB->idx];
11614 }
11615 }
11616 } else {
11617 localB->g_idx_0 = obj->ConstraintBound->size[0];
11618 localB->b_i_l = activeSet->size[0];
11619 activeSet->size[0] = localB->g_idx_0;
11620 car_emxEnsureCapacity_boolean_T(activeSet, localB->b_i_l);
11621 for (localB->b_i_l = 0; localB->b_i_l < localB->g_idx_0; localB->b_i_l++) {
11622 activeSet->data[localB->b_i_l] = false;
11623 }
11624
11625 A->size[0] = 6;
11626 A->size[1] = 0;
11627 }
11628
11629 localB->j = A->size[1] - 1;
11630 cartesian_trajec_emxInit_real_T(&AIn, 2, localB);
11631 cartesian_trajec_emxInit_real_T(&A_0, 2, localB);
11632 cartesian_trajec_emxInit_real_T(&A_1, 1, localB);
11633 for (localB->nx_n = 0; localB->nx_n <= localB->j; localB->nx_n++) {
11634 localB->i_i = A->size[0];
11635 localB->b_i_l = A_0->size[0] * A_0->size[1];
11636 A_0->size[0] = 1;
11637 A_0->size[1] = localB->i_i;
11638 cartes_emxEnsureCapacity_real_T(A_0, localB->b_i_l, localB);
11639 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l++) {
11640 A_0->data[localB->b_i_l] = A->data[A->size[0] * localB->nx_n +
11641 localB->b_i_l];
11642 }
11643
11644 localB->i_i = A->size[0];
11645 localB->b_i_l = A_1->size[0];
11646 A_1->size[0] = localB->i_i;
11647 cartes_emxEnsureCapacity_real_T(A_1, localB->b_i_l, localB);
11648 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l++) {
11649 A_1->data[localB->b_i_l] = A->data[A->size[0] * localB->nx_n +
11650 localB->b_i_l];
11651 }
11652
11653 localB->A_f = 0.0;
11654 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
11655 localB->Hg[localB->b_i_l] = 0.0;
11656 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
11657 localB->s_j = localB->H[6 * localB->b_i_l + localB->idx] * A_0->
11658 data[localB->idx] + localB->Hg[localB->b_i_l];
11659 localB->Hg[localB->b_i_l] = localB->s_j;
11660 }
11661
11662 localB->A_f += localB->Hg[localB->b_i_l] * A_1->data[localB->b_i_l];
11663 }
11664
11665 localB->s_j = 1.0 / localB->A_f;
11666 for (localB->b_i_l = 0; localB->b_i_l < 36; localB->b_i_l++) {
11667 localB->V[localB->b_i_l] = localB->s_j * localB->H[localB->b_i_l];
11668 }
11669
11670 localB->i_i = A->size[0];
11671 localB->n = A->size[0];
11672 localB->b_i_l = AIn->size[0] * AIn->size[1];
11673 AIn->size[0] = localB->i_i;
11674 AIn->size[1] = localB->n;
11675 cartes_emxEnsureCapacity_real_T(AIn, localB->b_i_l, localB);
11676 for (localB->b_i_l = 0; localB->b_i_l < localB->n; localB->b_i_l++) {
11677 for (localB->idx = 0; localB->idx < localB->i_i; localB->idx++) {
11678 AIn->data[localB->idx + AIn->size[0] * localB->b_i_l] = A->data[A->size
11679 [0] * localB->nx_n + localB->idx] * A->data[A->size[0] * localB->nx_n
11680 + localB->b_i_l];
11681 }
11682 }
11683
11684 localB->n = AIn->size[1] - 1;
11685 localB->b_i_l = unusedU1->size[0] * unusedU1->size[1];
11686 unusedU1->size[0] = 6;
11687 unusedU1->size[1] = AIn->size[1];
11688 cartes_emxEnsureCapacity_real_T(unusedU1, localB->b_i_l, localB);
11689 for (localB->idx = 0; localB->idx <= localB->n; localB->idx++) {
11690 localB->coffset = localB->idx * 6 - 1;
11691 localB->boffset = localB->idx * AIn->size[0] - 1;
11692 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
11693 localB->s_j = 0.0;
11694 for (localB->g_idx_0 = 0; localB->g_idx_0 < 6; localB->g_idx_0++) {
11695 localB->s_j += localB->V[localB->g_idx_0 * 6 + localB->b_i_l] *
11696 AIn->data[(localB->boffset + localB->g_idx_0) + 1];
11697 }
11698
11699 unusedU1->data[(localB->coffset + localB->b_i_l) + 1] = localB->s_j;
11700 }
11701 }
11702
11703 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
11704 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
11705 localB->s_j = 0.0;
11706 for (localB->i_i = 0; localB->i_i < 6; localB->i_i++) {
11707 localB->s_j += unusedU1->data[6 * localB->i_i + localB->b_i_l] *
11708 localB->H[6 * localB->idx + localB->i_i];
11709 }
11710
11711 localB->idxl = 6 * localB->idx + localB->b_i_l;
11712 localB->H_m[localB->idxl] = localB->H[localB->idxl] - localB->s_j;
11713 }
11714 }
11715
11716 memcpy(&localB->H[0], &localB->H_m[0], 36U * sizeof(real_T));
11717 }
11718
11719 cartesian_trajec_emxFree_real_T(&A_1);
11720 cartesian_trajec_emxFree_real_T(&A_0);
11721 for (localB->i_i = 0; localB->i_i < 6; localB->i_i++) {
11722 xSol[localB->i_i] = localB->x[localB->i_i];
11723 }
11724
11725 localB->A_f = obj->MaxNumIterationInternal;
11726 localB->g_idx_0 = 0;
11727 cartesian_trajec_emxInit_real_T(&L, 1, localB);
11728 cartesian_traje_emxInit_int32_T(&cb, 1);
11729 cartesian_traje_emxInit_int32_T(&db, 1);
11730 cartesian_traje_emxInit_int32_T(&eb, 1);
11731 cartesian_traje_emxInit_int32_T(&fb, 1);
11732 cartesian_traje_emxInit_int32_T(&gb, 1);
11733 cartesian_trajec_emxInit_real_T(&a, 2, localB);
11734 cartesian_trajec_emxInit_real_T(&y, 1, localB);
11735 cartesian_traje_emxInit_int32_T(&ii_0, 1);
11736 cartesian_trajec_emxInit_real_T(&y_0, 2, localB);
11737 cartesian_tra_emxInit_boolean_T(&x, 1);
11738 cartesian_trajec_emxInit_real_T(&A_2, 2, localB);
11739 cartesian_trajec_emxInit_real_T(&sigma, 2, localB);
11740 cartesian_trajec_emxInit_real_T(&tmp, 2, localB);
11741 cartesian_trajec_emxInit_real_T(&tmp_0, 2, localB);
11742 cartesian_trajec_emxInit_real_T(&grad_0, 2, localB);
11743 cartesian_trajec_emxInit_real_T(&sNew, 2, localB);
11744 cartesian_traje_emxInit_int32_T(&ii_1, 1);
11745 cartesian_traje_emxInit_int32_T(&ii_2, 1);
11746 cartesian_trajec_emxInit_real_T(&grad_1, 2, localB);
11747 cartesian_trajec_emxInit_real_T(&A_3, 2, localB);
11748 cartesian_trajec_emxInit_real_T(&alpha_0, 2, localB);
11749 cartesian_traje_emxInit_int32_T(&ii_3, 1);
11750 do {
11751 exitg2 = 0;
11752 if (localB->g_idx_0 <= static_cast<int32_T>(localB->A_f) - 1) {
11753 localB->s_j = SystemTimeProvider_getElapsedTi(&obj->TimeObjInternal);
11754 localB->flag = (localB->s_j > obj->MaxTimeInternal);
11755 if (localB->flag) {
11756 *exitFlag = TimeLimitExceeded;
11757 args = obj->ExtraArgs;
11758 for (localB->b_i_l = 0; localB->b_i_l < 36; localB->b_i_l++) {
11759 localB->unusedU0[localB->b_i_l] = args->WeightMatrix[localB->b_i_l];
11760 }
11761
11762 localB->b_i_l = grad->size[0];
11763 grad->size[0] = args->ErrTemp->size[0];
11764 cartes_emxEnsureCapacity_real_T(grad, localB->b_i_l, localB);
11765 localB->i_i = args->ErrTemp->size[0];
11766 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l++) {
11767 grad->data[localB->b_i_l] = args->ErrTemp->data[localB->b_i_l];
11768 }
11769
11770 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
11771 localB->x[localB->b_i_l] = 0.0;
11772 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
11773 localB->A_f = localB->unusedU0[6 * localB->idx + localB->b_i_l] *
11774 grad->data[localB->idx] + localB->x[localB->b_i_l];
11775 localB->x[localB->b_i_l] = localB->A_f;
11776 }
11777 }
11778
11779 *err = cartesian_trajectory_pla_norm_j(localB->x);
11780 *iter = static_cast<real_T>(localB->g_idx_0) + 1.0;
11781 exitg2 = 1;
11782 } else {
11783 if ((A->size[0] == 0) || (A->size[1] == 0)) {
11784 localB->b_i_l = alpha->size[0];
11785 alpha->size[0] = 1;
11786 cartes_emxEnsureCapacity_real_T(alpha, localB->b_i_l, localB);
11787 alpha->data[0] = 0.0;
11788 } else {
11789 localB->m_i = A->size[1] - 1;
11790 localB->inner = A->size[0] - 1;
11791 localB->n = A->size[1] - 1;
11792 localB->b_i_l = AIn->size[0] * AIn->size[1];
11793 AIn->size[0] = A->size[1];
11794 AIn->size[1] = A->size[1];
11795 cartes_emxEnsureCapacity_real_T(AIn, localB->b_i_l, localB);
11796 for (localB->idx = 0; localB->idx <= localB->n; localB->idx++) {
11797 localB->coffset = (localB->m_i + 1) * localB->idx - 1;
11798 localB->boffset = localB->idx * A->size[0] - 1;
11799 for (localB->b_i_l = 0; localB->b_i_l <= localB->m_i; localB->b_i_l
11800 ++) {
11801 AIn->data[(localB->coffset + localB->b_i_l) + 1] = 0.0;
11802 }
11803
11804 for (localB->nx_n = 0; localB->nx_n <= localB->inner; localB->nx_n++)
11805 {
11806 localB->s_j = A->data[(localB->boffset + localB->nx_n) + 1];
11807 for (localB->j = 0; localB->j <= localB->m_i; localB->j++) {
11808 localB->b_i_l = (localB->coffset + localB->j) + 1;
11809 AIn->data[localB->b_i_l] += A->data[localB->j * A->size[0] +
11810 localB->nx_n] * localB->s_j;
11811 }
11812 }
11813 }
11814
11815 localB->b_i_l = A_2->size[0] * A_2->size[1];
11816 A_2->size[0] = A->size[1];
11817 A_2->size[1] = A->size[0];
11818 cartes_emxEnsureCapacity_real_T(A_2, localB->b_i_l, localB);
11819 localB->i_i = A->size[0];
11820 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l++)
11821 {
11822 localB->n = A->size[1];
11823 for (localB->idx = 0; localB->idx < localB->n; localB->idx++) {
11824 A_2->data[localB->idx + A_2->size[0] * localB->b_i_l] = A->data
11825 [A->size[0] * localB->idx + localB->b_i_l];
11826 }
11827 }
11828
11829 cartesian_trajectory_p_mldivide(AIn, A_2, a, localB);
11830 localB->m_i = a->size[0] - 1;
11831 localB->inner = a->size[1] - 1;
11832 localB->b_i_l = alpha->size[0];
11833 alpha->size[0] = a->size[0];
11834 cartes_emxEnsureCapacity_real_T(alpha, localB->b_i_l, localB);
11835 for (localB->b_i_l = 0; localB->b_i_l <= localB->m_i; localB->b_i_l++)
11836 {
11837 alpha->data[localB->b_i_l] = 0.0;
11838 }
11839
11840 for (localB->nx_n = 0; localB->nx_n <= localB->inner; localB->nx_n++)
11841 {
11842 localB->aoffset = localB->nx_n * a->size[0] - 1;
11843 for (localB->j = 0; localB->j <= localB->m_i; localB->j++) {
11844 alpha->data[localB->j] += a->data[(localB->aoffset + localB->j) +
11845 1] * grad->data[localB->nx_n];
11846 }
11847 }
11848 }
11849
11850 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
11851 localB->Hg[localB->b_i_l] = 0.0;
11852 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
11853 localB->b_gamma = localB->H[6 * localB->idx + localB->b_i_l] *
11854 grad->data[localB->idx] + localB->Hg[localB->b_i_l];
11855 localB->Hg[localB->b_i_l] = localB->b_gamma;
11856 }
11857 }
11858
11859 if (DampedBFGSwGradientProjection_a(obj, localB->Hg, alpha)) {
11860 *exitFlag = LocalMinimumFound;
11861 args = obj->ExtraArgs;
11862 for (localB->b_i_l = 0; localB->b_i_l < 36; localB->b_i_l++) {
11863 localB->unusedU0[localB->b_i_l] = args->WeightMatrix[localB->b_i_l];
11864 }
11865
11866 localB->b_i_l = grad->size[0];
11867 grad->size[0] = args->ErrTemp->size[0];
11868 cartes_emxEnsureCapacity_real_T(grad, localB->b_i_l, localB);
11869 localB->i_i = args->ErrTemp->size[0];
11870 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l++)
11871 {
11872 grad->data[localB->b_i_l] = args->ErrTemp->data[localB->b_i_l];
11873 }
11874
11875 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
11876 localB->x[localB->b_i_l] = 0.0;
11877 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
11878 localB->A_f = localB->unusedU0[6 * localB->idx + localB->b_i_l] *
11879 grad->data[localB->idx] + localB->x[localB->b_i_l];
11880 localB->x[localB->b_i_l] = localB->A_f;
11881 }
11882 }
11883
11884 *err = cartesian_trajectory_pla_norm_j(localB->x);
11885 *iter = static_cast<real_T>(localB->g_idx_0) + 1.0;
11886 exitg2 = 1;
11887 } else {
11888 guard1 = false;
11889 guard2 = false;
11890 if (obj->ConstraintsOn && ((A->size[0] != 0) && (A->size[1] != 0))) {
11891 localB->m_i = A->size[1] - 1;
11892 localB->inner = A->size[0] - 1;
11893 localB->n = A->size[1] - 1;
11894 localB->b_i_l = AIn->size[0] * AIn->size[1];
11895 AIn->size[0] = A->size[1];
11896 AIn->size[1] = A->size[1];
11897 cartes_emxEnsureCapacity_real_T(AIn, localB->b_i_l, localB);
11898 for (localB->idx = 0; localB->idx <= localB->n; localB->idx++) {
11899 localB->coffset = (localB->m_i + 1) * localB->idx - 1;
11900 localB->boffset = localB->idx * A->size[0] - 1;
11901 for (localB->b_i_l = 0; localB->b_i_l <= localB->m_i;
11902 localB->b_i_l++) {
11903 AIn->data[(localB->coffset + localB->b_i_l) + 1] = 0.0;
11904 }
11905
11906 for (localB->nx_n = 0; localB->nx_n <= localB->inner; localB->nx_n
11907 ++) {
11908 localB->s_j = A->data[(localB->boffset + localB->nx_n) + 1];
11909 for (localB->j = 0; localB->j <= localB->m_i; localB->j++) {
11910 localB->b_i_l = (localB->coffset + localB->j) + 1;
11911 AIn->data[localB->b_i_l] += A->data[localB->j * A->size[0] +
11912 localB->nx_n] * localB->s_j;
11913 }
11914 }
11915 }
11916
11917 cartesian_trajectory_planne_inv(AIn, a, localB);
11918 cartesian_trajectory_plann_diag(a, L, localB);
11919 cartesian_trajectory_pl_sqrt_nh(L);
11920 localB->b_i_l = alpha->size[0];
11921 cartes_emxEnsureCapacity_real_T(alpha, localB->b_i_l, localB);
11922 localB->i_i = alpha->size[0];
11923 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l++)
11924 {
11925 alpha->data[localB->b_i_l] /= L->data[localB->b_i_l];
11926 }
11927
11928 localB->n = alpha->size[0];
11929 if (alpha->size[0] <= 2) {
11930 if (alpha->size[0] == 1) {
11931 localB->s_j = alpha->data[0];
11932 localB->idxl = 0;
11933 } else if ((alpha->data[0] < alpha->data[1]) || (rtIsNaN
11934 (alpha->data[0]) && (!rtIsNaN(alpha->data[1])))) {
11935 localB->s_j = alpha->data[1];
11936 localB->idxl = 1;
11937 } else {
11938 localB->s_j = alpha->data[0];
11939 localB->idxl = 0;
11940 }
11941 } else {
11942 if (!rtIsNaN(alpha->data[0])) {
11943 localB->idxl = 1;
11944 } else {
11945 localB->idxl = 0;
11946 localB->b_i_l = 2;
11947 exitg3 = false;
11948 while ((!exitg3) && (localB->b_i_l <= alpha->size[0])) {
11949 if (!rtIsNaN(alpha->data[localB->b_i_l - 1])) {
11950 localB->idxl = localB->b_i_l;
11951 exitg3 = true;
11952 } else {
11953 localB->b_i_l++;
11954 }
11955 }
11956 }
11957
11958 if (localB->idxl == 0) {
11959 localB->s_j = alpha->data[0];
11960 } else {
11961 localB->s_j = alpha->data[localB->idxl - 1];
11962 localB->nx_n = localB->idxl;
11963 for (localB->b_i_l = localB->idxl + 1; localB->b_i_l <=
11964 localB->n; localB->b_i_l++) {
11965 if (localB->s_j < alpha->data[localB->b_i_l - 1]) {
11966 localB->s_j = alpha->data[localB->b_i_l - 1];
11967 localB->nx_n = localB->b_i_l;
11968 }
11969 }
11970
11971 localB->idxl = localB->nx_n - 1;
11972 }
11973 }
11974
11975 if (cartesian_trajectory_pla_norm_j(localB->Hg) < 0.5 * localB->s_j)
11976 {
11977 localB->nx_n = activeSet->size[0];
11978 localB->idx = 0;
11979 localB->b_i_l = ii->size[0];
11980 ii->size[0] = activeSet->size[0];
11981 carte_emxEnsureCapacity_int32_T(ii, localB->b_i_l);
11982 localB->b_i_l = 1;
11983 exitg3 = false;
11984 while ((!exitg3) && (localB->b_i_l - 1 <= localB->nx_n - 1)) {
11985 if (activeSet->data[localB->b_i_l - 1]) {
11986 localB->idx++;
11987 ii->data[localB->idx - 1] = localB->b_i_l;
11988 if (localB->idx >= localB->nx_n) {
11989 exitg3 = true;
11990 } else {
11991 localB->b_i_l++;
11992 }
11993 } else {
11994 localB->b_i_l++;
11995 }
11996 }
11997
11998 if (activeSet->size[0] == 1) {
11999 if (localB->idx == 0) {
12000 ii->size[0] = 0;
12001 }
12002 } else {
12003 if (1 > localB->idx) {
12004 localB->idx = 0;
12005 }
12006
12007 localB->b_i_l = ii_1->size[0];
12008 ii_1->size[0] = localB->idx;
12009 carte_emxEnsureCapacity_int32_T(ii_1, localB->b_i_l);
12010 for (localB->b_i_l = 0; localB->b_i_l < localB->idx;
12011 localB->b_i_l++) {
12012 ii_1->data[localB->b_i_l] = ii->data[localB->b_i_l];
12013 }
12014
12015 localB->b_i_l = ii->size[0];
12016 ii->size[0] = ii_1->size[0];
12017 carte_emxEnsureCapacity_int32_T(ii, localB->b_i_l);
12018 localB->i_i = ii_1->size[0];
12019 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i;
12020 localB->b_i_l++) {
12021 ii->data[localB->b_i_l] = ii_1->data[localB->b_i_l];
12022 }
12023 }
12024
12025 localB->b_i_l = alpha->size[0];
12026 alpha->size[0] = ii->size[0];
12027 cartes_emxEnsureCapacity_real_T(alpha, localB->b_i_l, localB);
12028 localB->i_i = ii->size[0];
12029 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l
12030 ++) {
12031 alpha->data[localB->b_i_l] = ii->data[localB->b_i_l];
12032 }
12033
12034 activeSet->data[static_cast<int32_T>(alpha->data[localB->idxl]) -
12035 1] = false;
12036 localB->nx_n = activeSet->size[0] - 1;
12037 localB->idx = 0;
12038 for (localB->b_i_l = 0; localB->b_i_l <= localB->nx_n;
12039 localB->b_i_l++) {
12040 if (activeSet->data[localB->b_i_l]) {
12041 localB->idx++;
12042 }
12043 }
12044
12045 localB->b_i_l = eb->size[0];
12046 eb->size[0] = localB->idx;
12047 carte_emxEnsureCapacity_int32_T(eb, localB->b_i_l);
12048 localB->idx = 0;
12049 for (localB->b_i_l = 0; localB->b_i_l <= localB->nx_n;
12050 localB->b_i_l++) {
12051 if (activeSet->data[localB->b_i_l]) {
12052 eb->data[localB->idx] = localB->b_i_l + 1;
12053 localB->idx++;
12054 }
12055 }
12056
12057 localB->i_i = obj->ConstraintMatrix->size[0];
12058 localB->b_i_l = A->size[0] * A->size[1];
12059 A->size[0] = localB->i_i;
12060 A->size[1] = eb->size[0];
12061 cartes_emxEnsureCapacity_real_T(A, localB->b_i_l, localB);
12062 localB->n = eb->size[0];
12063 for (localB->b_i_l = 0; localB->b_i_l < localB->n; localB->b_i_l++)
12064 {
12065 for (localB->idx = 0; localB->idx < localB->i_i; localB->idx++)
12066 {
12067 A->data[localB->idx + A->size[0] * localB->b_i_l] =
12068 obj->ConstraintMatrix->data[(eb->data[localB->b_i_l] - 1) *
12069 obj->ConstraintMatrix->size[0] + localB->idx];
12070 }
12071 }
12072
12073 localB->m_i = A->size[1] - 1;
12074 localB->inner = A->size[0] - 1;
12075 localB->n = A->size[1] - 1;
12076 localB->b_i_l = AIn->size[0] * AIn->size[1];
12077 AIn->size[0] = A->size[1];
12078 AIn->size[1] = A->size[1];
12079 cartes_emxEnsureCapacity_real_T(AIn, localB->b_i_l, localB);
12080 for (localB->idx = 0; localB->idx <= localB->n; localB->idx++) {
12081 localB->coffset = (localB->m_i + 1) * localB->idx - 1;
12082 localB->boffset = localB->idx * A->size[0] - 1;
12083 for (localB->b_i_l = 0; localB->b_i_l <= localB->m_i;
12084 localB->b_i_l++) {
12085 AIn->data[(localB->coffset + localB->b_i_l) + 1] = 0.0;
12086 }
12087
12088 for (localB->nx_n = 0; localB->nx_n <= localB->inner;
12089 localB->nx_n++) {
12090 localB->s_j = A->data[(localB->boffset + localB->nx_n) + 1];
12091 for (localB->j = 0; localB->j <= localB->m_i; localB->j++) {
12092 localB->b_i_l = (localB->coffset + localB->j) + 1;
12093 AIn->data[localB->b_i_l] += A->data[localB->j * A->size[0] +
12094 localB->nx_n] * localB->s_j;
12095 }
12096 }
12097 }
12098
12099 localB->b_i_l = A_3->size[0] * A_3->size[1];
12100 A_3->size[0] = A->size[1];
12101 A_3->size[1] = A->size[0];
12102 cartes_emxEnsureCapacity_real_T(A_3, localB->b_i_l, localB);
12103 localB->i_i = A->size[0];
12104 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l
12105 ++) {
12106 localB->n = A->size[1];
12107 for (localB->idx = 0; localB->idx < localB->n; localB->idx++) {
12108 A_3->data[localB->idx + A_3->size[0] * localB->b_i_l] =
12109 A->data[A->size[0] * localB->idx + localB->b_i_l];
12110 }
12111 }
12112
12113 cartesian_trajectory_p_mldivide(AIn, A_3, a, localB);
12114 localB->m_i = A->size[0] - 1;
12115 localB->inner = A->size[1] - 1;
12116 localB->n = a->size[1] - 1;
12117 localB->b_i_l = AIn->size[0] * AIn->size[1];
12118 AIn->size[0] = A->size[0];
12119 AIn->size[1] = a->size[1];
12120 cartes_emxEnsureCapacity_real_T(AIn, localB->b_i_l, localB);
12121 for (localB->idx = 0; localB->idx <= localB->n; localB->idx++) {
12122 localB->coffset = (localB->m_i + 1) * localB->idx - 1;
12123 localB->boffset = localB->idx * a->size[0] - 1;
12124 for (localB->b_i_l = 0; localB->b_i_l <= localB->m_i;
12125 localB->b_i_l++) {
12126 AIn->data[(localB->coffset + localB->b_i_l) + 1] = 0.0;
12127 }
12128
12129 for (localB->nx_n = 0; localB->nx_n <= localB->inner;
12130 localB->nx_n++) {
12131 localB->aoffset = localB->nx_n * A->size[0] - 1;
12132 localB->s_j = a->data[(localB->boffset + localB->nx_n) + 1];
12133 for (localB->j = 0; localB->j <= localB->m_i; localB->j++) {
12134 localB->i_i = localB->j + 1;
12135 localB->b_i_l = localB->coffset + localB->i_i;
12136 AIn->data[localB->b_i_l] += A->data[localB->aoffset +
12137 localB->i_i] * localB->s_j;
12138 }
12139 }
12140 }
12141
12142 for (localB->b_i_l = 0; localB->b_i_l < 36; localB->b_i_l++) {
12143 localB->P[localB->b_i_l] = localB->unusedU0[localB->b_i_l] -
12144 AIn->data[localB->b_i_l];
12145 }
12146
12147 localB->s_j = alpha->data[localB->idxl];
12148 localB->b_i_l = static_cast<int32_T>(localB->s_j);
12149 localB->i_i = obj->ConstraintMatrix->size[0];
12150 localB->idx = alpha->size[0];
12151 alpha->size[0] = localB->i_i;
12152 cartes_emxEnsureCapacity_real_T(alpha, localB->idx, localB);
12153 for (localB->idx = 0; localB->idx < localB->i_i; localB->idx++) {
12154 alpha->data[localB->idx] = obj->ConstraintMatrix->data
12155 [(localB->b_i_l - 1) * obj->ConstraintMatrix->size[0] +
12156 localB->idx];
12157 }
12158
12159 localB->b_i_l = alpha_0->size[0] * alpha_0->size[1];
12160 alpha_0->size[0] = 1;
12161 alpha_0->size[1] = alpha->size[0];
12162 cartes_emxEnsureCapacity_real_T(alpha_0, localB->b_i_l, localB);
12163 localB->i_i = alpha->size[0];
12164 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l
12165 ++) {
12166 alpha_0->data[localB->b_i_l] = alpha->data[localB->b_i_l];
12167 }
12168
12169 localB->s_j = 0.0;
12170 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12171 localB->Hg[localB->b_i_l] = 0.0;
12172 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
12173 localB->b_gamma = localB->P[6 * localB->b_i_l + localB->idx] *
12174 alpha_0->data[localB->idx] + localB->Hg[localB->b_i_l];
12175 localB->Hg[localB->b_i_l] = localB->b_gamma;
12176 }
12177
12178 localB->s_j += localB->Hg[localB->b_i_l] * alpha->data
12179 [localB->b_i_l];
12180 }
12181
12182 localB->s_j = 1.0 / localB->s_j;
12183 for (localB->b_i_l = 0; localB->b_i_l < 36; localB->b_i_l++) {
12184 localB->V[localB->b_i_l] = localB->s_j * localB->P[localB->b_i_l];
12185 }
12186
12187 localB->b_i_l = AIn->size[0] * AIn->size[1];
12188 AIn->size[0] = alpha->size[0];
12189 AIn->size[1] = alpha->size[0];
12190 cartes_emxEnsureCapacity_real_T(AIn, localB->b_i_l, localB);
12191 localB->i_i = alpha->size[0];
12192 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l
12193 ++) {
12194 localB->n = alpha->size[0];
12195 for (localB->idx = 0; localB->idx < localB->n; localB->idx++) {
12196 AIn->data[localB->idx + AIn->size[0] * localB->b_i_l] =
12197 alpha->data[localB->idx] * alpha->data[localB->b_i_l];
12198 }
12199 }
12200
12201 localB->n = AIn->size[1] - 1;
12202 localB->b_i_l = unusedU1->size[0] * unusedU1->size[1];
12203 unusedU1->size[0] = 6;
12204 unusedU1->size[1] = AIn->size[1];
12205 cartes_emxEnsureCapacity_real_T(unusedU1, localB->b_i_l, localB);
12206 for (localB->idx = 0; localB->idx <= localB->n; localB->idx++) {
12207 localB->coffset = localB->idx * 6 - 1;
12208 localB->boffset = localB->idx * AIn->size[0] - 1;
12209 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12210 localB->s_j = 0.0;
12211 for (localB->nx_n = 0; localB->nx_n < 6; localB->nx_n++) {
12212 localB->s_j += localB->V[localB->nx_n * 6 + localB->b_i_l] *
12213 AIn->data[(localB->boffset + localB->nx_n) + 1];
12214 }
12215
12216 unusedU1->data[(localB->coffset + localB->b_i_l) + 1] =
12217 localB->s_j;
12218 }
12219 }
12220
12221 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12222 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
12223 localB->s_j = 0.0;
12224 for (localB->i_i = 0; localB->i_i < 6; localB->i_i++) {
12225 localB->s_j += unusedU1->data[6 * localB->i_i +
12226 localB->b_i_l] * localB->P[6 * localB->idx + localB->i_i];
12227 }
12228
12229 localB->idxl = 6 * localB->idx + localB->b_i_l;
12230 localB->H[localB->idxl] += localB->s_j;
12231 }
12232 }
12233
12234 localB->g_idx_0++;
12235 } else {
12236 guard2 = true;
12237 }
12238 } else {
12239 guard2 = true;
12240 }
12241
12242 if (guard2) {
12243 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12244 localB->Hg[localB->b_i_l] = -localB->Hg[localB->b_i_l];
12245 }
12246
12247 localB->idxl = -1;
12248 if (obj->ConstraintsOn) {
12249 localB->b_i_l = x->size[0];
12250 x->size[0] = activeSet->size[0];
12251 car_emxEnsureCapacity_boolean_T(x, localB->b_i_l);
12252 localB->i_i = activeSet->size[0];
12253 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l
12254 ++) {
12255 x->data[localB->b_i_l] = !activeSet->data[localB->b_i_l];
12256 }
12257
12258 if (cartesian_trajectory_planne_any(x)) {
12259 localB->nx_n = activeSet->size[0] - 1;
12260 localB->idx = 0;
12261 for (localB->b_i_l = 0; localB->b_i_l <= localB->nx_n;
12262 localB->b_i_l++) {
12263 if (!activeSet->data[localB->b_i_l]) {
12264 localB->idx++;
12265 }
12266 }
12267
12268 localB->b_i_l = cb->size[0];
12269 cb->size[0] = localB->idx;
12270 carte_emxEnsureCapacity_int32_T(cb, localB->b_i_l);
12271 localB->idx = 0;
12272 for (localB->b_i_l = 0; localB->b_i_l <= localB->nx_n;
12273 localB->b_i_l++) {
12274 if (!activeSet->data[localB->b_i_l]) {
12275 cb->data[localB->idx] = localB->b_i_l + 1;
12276 localB->idx++;
12277 }
12278 }
12279
12280 localB->b_i_l = alpha->size[0];
12281 alpha->size[0] = cb->size[0];
12282 cartes_emxEnsureCapacity_real_T(alpha, localB->b_i_l, localB);
12283 localB->i_i = cb->size[0];
12284 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i;
12285 localB->b_i_l++) {
12286 alpha->data[localB->b_i_l] = obj->ConstraintBound->data
12287 [cb->data[localB->b_i_l] - 1];
12288 }
12289
12290 localB->nx_n = activeSet->size[0] - 1;
12291 localB->idx = 0;
12292 for (localB->b_i_l = 0; localB->b_i_l <= localB->nx_n;
12293 localB->b_i_l++) {
12294 if (!activeSet->data[localB->b_i_l]) {
12295 localB->idx++;
12296 }
12297 }
12298
12299 localB->b_i_l = db->size[0];
12300 db->size[0] = localB->idx;
12301 carte_emxEnsureCapacity_int32_T(db, localB->b_i_l);
12302 localB->idx = 0;
12303 for (localB->b_i_l = 0; localB->b_i_l <= localB->nx_n;
12304 localB->b_i_l++) {
12305 if (!activeSet->data[localB->b_i_l]) {
12306 db->data[localB->idx] = localB->b_i_l + 1;
12307 localB->idx++;
12308 }
12309 }
12310
12311 localB->i_i = obj->ConstraintMatrix->size[0];
12312 localB->b_i_l = AIn->size[0] * AIn->size[1];
12313 AIn->size[0] = localB->i_i;
12314 AIn->size[1] = db->size[0];
12315 cartes_emxEnsureCapacity_real_T(AIn, localB->b_i_l, localB);
12316 localB->n = db->size[0];
12317 for (localB->b_i_l = 0; localB->b_i_l < localB->n; localB->b_i_l
12318 ++) {
12319 for (localB->idx = 0; localB->idx < localB->i_i; localB->idx++)
12320 {
12321 AIn->data[localB->idx + AIn->size[0] * localB->b_i_l] =
12322 obj->ConstraintMatrix->data[(db->data[localB->b_i_l] - 1) *
12323 obj->ConstraintMatrix->size[0] + localB->idx];
12324 }
12325 }
12326
12327 localB->nx_n = x->size[0];
12328 localB->idx = 0;
12329 localB->b_i_l = ii->size[0];
12330 ii->size[0] = x->size[0];
12331 carte_emxEnsureCapacity_int32_T(ii, localB->b_i_l);
12332 localB->b_i_l = 1;
12333 exitg3 = false;
12334 while ((!exitg3) && (localB->b_i_l - 1 <= localB->nx_n - 1)) {
12335 if (x->data[localB->b_i_l - 1]) {
12336 localB->idx++;
12337 ii->data[localB->idx - 1] = localB->b_i_l;
12338 if (localB->idx >= localB->nx_n) {
12339 exitg3 = true;
12340 } else {
12341 localB->b_i_l++;
12342 }
12343 } else {
12344 localB->b_i_l++;
12345 }
12346 }
12347
12348 if (x->size[0] == 1) {
12349 if (localB->idx == 0) {
12350 ii->size[0] = 0;
12351 }
12352 } else {
12353 if (1 > localB->idx) {
12354 localB->idx = 0;
12355 }
12356
12357 localB->b_i_l = ii_2->size[0];
12358 ii_2->size[0] = localB->idx;
12359 carte_emxEnsureCapacity_int32_T(ii_2, localB->b_i_l);
12360 for (localB->b_i_l = 0; localB->b_i_l < localB->idx;
12361 localB->b_i_l++) {
12362 ii_2->data[localB->b_i_l] = ii->data[localB->b_i_l];
12363 }
12364
12365 localB->b_i_l = ii->size[0];
12366 ii->size[0] = ii_2->size[0];
12367 carte_emxEnsureCapacity_int32_T(ii, localB->b_i_l);
12368 localB->i_i = ii_2->size[0];
12369 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i;
12370 localB->b_i_l++) {
12371 ii->data[localB->b_i_l] = ii_2->data[localB->b_i_l];
12372 }
12373 }
12374
12375 localB->m_i = AIn->size[1] - 1;
12376 localB->inner = AIn->size[0] - 1;
12377 localB->b_i_l = L->size[0];
12378 L->size[0] = AIn->size[1];
12379 cartes_emxEnsureCapacity_real_T(L, localB->b_i_l, localB);
12380 for (localB->b_i_l = 0; localB->b_i_l <= localB->m_i;
12381 localB->b_i_l++) {
12382 L->data[localB->b_i_l] = 0.0;
12383 }
12384
12385 for (localB->nx_n = 0; localB->nx_n <= localB->inner;
12386 localB->nx_n++) {
12387 for (localB->j = 0; localB->j <= localB->m_i; localB->j++) {
12388 L->data[localB->j] += AIn->data[localB->j * AIn->size[0] +
12389 localB->nx_n] * localB->x[localB->nx_n];
12390 }
12391 }
12392
12393 localB->m_i = AIn->size[1] - 1;
12394 localB->inner = AIn->size[0] - 1;
12395 localB->b_i_l = y->size[0];
12396 y->size[0] = AIn->size[1];
12397 cartes_emxEnsureCapacity_real_T(y, localB->b_i_l, localB);
12398 for (localB->b_i_l = 0; localB->b_i_l <= localB->m_i;
12399 localB->b_i_l++) {
12400 y->data[localB->b_i_l] = 0.0;
12401 }
12402
12403 for (localB->nx_n = 0; localB->nx_n <= localB->inner;
12404 localB->nx_n++) {
12405 for (localB->j = 0; localB->j <= localB->m_i; localB->j++) {
12406 y->data[localB->j] += AIn->data[localB->j * AIn->size[0] +
12407 localB->nx_n] * localB->Hg[localB->nx_n];
12408 }
12409 }
12410
12411 localB->b_i_l = alpha->size[0];
12412 cartes_emxEnsureCapacity_real_T(alpha, localB->b_i_l, localB);
12413 localB->i_i = alpha->size[0];
12414 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i;
12415 localB->b_i_l++) {
12416 alpha->data[localB->b_i_l] = (alpha->data[localB->b_i_l] -
12417 L->data[localB->b_i_l]) / y->data[localB->b_i_l];
12418 }
12419
12420 localB->b_i_l = x->size[0];
12421 x->size[0] = alpha->size[0];
12422 car_emxEnsureCapacity_boolean_T(x, localB->b_i_l);
12423 localB->i_i = alpha->size[0];
12424 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i;
12425 localB->b_i_l++) {
12426 x->data[localB->b_i_l] = (alpha->data[localB->b_i_l] > 0.0);
12427 }
12428
12429 localB->nx_n = x->size[0];
12430 localB->idx = 0;
12431 localB->b_i_l = ii_0->size[0];
12432 ii_0->size[0] = x->size[0];
12433 carte_emxEnsureCapacity_int32_T(ii_0, localB->b_i_l);
12434 localB->b_i_l = 1;
12435 exitg3 = false;
12436 while ((!exitg3) && (localB->b_i_l - 1 <= localB->nx_n - 1)) {
12437 if (x->data[localB->b_i_l - 1]) {
12438 localB->idx++;
12439 ii_0->data[localB->idx - 1] = localB->b_i_l;
12440 if (localB->idx >= localB->nx_n) {
12441 exitg3 = true;
12442 } else {
12443 localB->b_i_l++;
12444 }
12445 } else {
12446 localB->b_i_l++;
12447 }
12448 }
12449
12450 if (x->size[0] == 1) {
12451 if (localB->idx == 0) {
12452 ii_0->size[0] = 0;
12453 }
12454 } else {
12455 if (1 > localB->idx) {
12456 localB->idx = 0;
12457 }
12458
12459 localB->b_i_l = ii_3->size[0];
12460 ii_3->size[0] = localB->idx;
12461 carte_emxEnsureCapacity_int32_T(ii_3, localB->b_i_l);
12462 for (localB->b_i_l = 0; localB->b_i_l < localB->idx;
12463 localB->b_i_l++) {
12464 ii_3->data[localB->b_i_l] = ii_0->data[localB->b_i_l];
12465 }
12466
12467 localB->b_i_l = ii_0->size[0];
12468 ii_0->size[0] = ii_3->size[0];
12469 carte_emxEnsureCapacity_int32_T(ii_0, localB->b_i_l);
12470 localB->i_i = ii_3->size[0];
12471 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i;
12472 localB->b_i_l++) {
12473 ii_0->data[localB->b_i_l] = ii_3->data[localB->b_i_l];
12474 }
12475 }
12476
12477 localB->b_i_l = L->size[0];
12478 L->size[0] = ii_0->size[0];
12479 cartes_emxEnsureCapacity_real_T(L, localB->b_i_l, localB);
12480 localB->i_i = ii_0->size[0];
12481 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i;
12482 localB->b_i_l++) {
12483 L->data[localB->b_i_l] = ii_0->data[localB->b_i_l];
12484 }
12485
12486 if (L->size[0] != 0) {
12487 localB->nx_n = alpha->size[0] - 1;
12488 localB->idx = 0;
12489 for (localB->b_i_l = 0; localB->b_i_l <= localB->nx_n;
12490 localB->b_i_l++) {
12491 if (alpha->data[localB->b_i_l] > 0.0) {
12492 localB->idx++;
12493 }
12494 }
12495
12496 localB->b_i_l = fb->size[0];
12497 fb->size[0] = localB->idx;
12498 carte_emxEnsureCapacity_int32_T(fb, localB->b_i_l);
12499 localB->idx = 0;
12500 for (localB->b_i_l = 0; localB->b_i_l <= localB->nx_n;
12501 localB->b_i_l++) {
12502 if (alpha->data[localB->b_i_l] > 0.0) {
12503 fb->data[localB->idx] = localB->b_i_l + 1;
12504 localB->idx++;
12505 }
12506 }
12507
12508 localB->n = fb->size[0];
12509 if (fb->size[0] <= 2) {
12510 if (fb->size[0] == 1) {
12511 localB->s_j = alpha->data[fb->data[0] - 1];
12512 localB->idxl = 0;
12513 } else if ((alpha->data[fb->data[0] - 1] > alpha->data
12514 [fb->data[1] - 1]) || (rtIsNaN(alpha->data
12515 [fb->data[0] - 1]) && (!rtIsNaN(alpha->data
12516 [fb->data[1] - 1])))) {
12517 localB->s_j = alpha->data[fb->data[1] - 1];
12518 localB->idxl = 1;
12519 } else {
12520 localB->s_j = alpha->data[fb->data[0] - 1];
12521 localB->idxl = 0;
12522 }
12523 } else {
12524 if (!rtIsNaN(alpha->data[fb->data[0] - 1])) {
12525 localB->idxl = 1;
12526 } else {
12527 localB->idxl = 0;
12528 localB->b_i_l = 2;
12529 exitg3 = false;
12530 while ((!exitg3) && (localB->b_i_l <= fb->size[0])) {
12531 if (!rtIsNaN(alpha->data[fb->data[localB->b_i_l - 1] - 1]))
12532 {
12533 localB->idxl = localB->b_i_l;
12534 exitg3 = true;
12535 } else {
12536 localB->b_i_l++;
12537 }
12538 }
12539 }
12540
12541 if (localB->idxl == 0) {
12542 localB->s_j = alpha->data[fb->data[0] - 1];
12543 } else {
12544 localB->s_j = alpha->data[fb->data[localB->idxl - 1] - 1];
12545 localB->nx_n = localB->idxl;
12546 for (localB->b_i_l = localB->idxl + 1; localB->b_i_l <=
12547 localB->n; localB->b_i_l++) {
12548 if (localB->s_j > alpha->data[fb->data[localB->b_i_l - 1]
12549 - 1]) {
12550 localB->s_j = alpha->data[fb->data[localB->b_i_l - 1]
12551 - 1];
12552 localB->nx_n = localB->b_i_l;
12553 }
12554 }
12555
12556 localB->idxl = localB->nx_n - 1;
12557 }
12558 }
12559
12560 localB->idxl = ii->data[static_cast<int32_T>(L->data
12561 [localB->idxl]) - 1];
12562 } else {
12563 localB->s_j = 0.0;
12564 }
12565 } else {
12566 localB->s_j = 0.0;
12567 }
12568 } else {
12569 localB->s_j = 0.0;
12570 }
12571
12572 if (localB->s_j > 0.0) {
12573 if (1.0 < localB->s_j) {
12574 localB->b_gamma = 1.0;
12575 } else {
12576 localB->b_gamma = localB->s_j;
12577 }
12578 } else {
12579 localB->b_gamma = 1.0;
12580 }
12581
12582 localB->beta = obj->ArmijoRuleBeta;
12583 localB->sigma = obj->ArmijoRuleSigma;
12584 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12585 localB->sNew_m[localB->b_i_l] = localB->b_gamma * localB->
12586 Hg[localB->b_i_l] + localB->x[localB->b_i_l];
12587 }
12588
12589 cartesian_IKHelpers_computeCost(localB->sNew_m, obj->ExtraArgs,
12590 &localB->costNew, localB->V, unusedU1, &c, localB);
12591 obj->ExtraArgs = c;
12592 localB->m = 0.0;
12593 do {
12594 exitg1 = 0;
12595 for (localB->i_i = 0; localB->i_i < 6; localB->i_i++) {
12596 xSol[localB->i_i] = localB->b_gamma * localB->Hg[localB->i_i];
12597 }
12598
12599 localB->b_i_l = sigma->size[0] * sigma->size[1];
12600 sigma->size[0] = 1;
12601 sigma->size[1] = grad->size[0];
12602 cartes_emxEnsureCapacity_real_T(sigma, localB->b_i_l, localB);
12603 localB->i_i = grad->size[0];
12604 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l
12605 ++) {
12606 sigma->data[localB->b_i_l] = -localB->sigma * grad->data
12607 [localB->b_i_l];
12608 }
12609
12610 localB->sigma_a = 0.0;
12611 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12612 localB->sigma_a += sigma->data[localB->b_i_l] * xSol
12613 [localB->b_i_l];
12614 }
12615
12616 if (localB->cost - localB->costNew < localB->sigma_a) {
12617 localB->flag = (localB->b_gamma < obj->StepTolerance);
12618 if (localB->flag) {
12619 for (localB->i_i = 0; localB->i_i < 6; localB->i_i++) {
12620 xSol[localB->i_i] = localB->x[localB->i_i];
12621 }
12622
12623 *exitFlag = StepSizeBelowMinimum;
12624 args = obj->ExtraArgs;
12625 for (localB->b_i_l = 0; localB->b_i_l < 36; localB->b_i_l++) {
12626 localB->unusedU0[localB->b_i_l] = args->WeightMatrix
12627 [localB->b_i_l];
12628 }
12629
12630 localB->b_i_l = grad->size[0];
12631 grad->size[0] = args->ErrTemp->size[0];
12632 cartes_emxEnsureCapacity_real_T(grad, localB->b_i_l, localB);
12633 localB->i_i = args->ErrTemp->size[0];
12634 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i;
12635 localB->b_i_l++) {
12636 grad->data[localB->b_i_l] = args->ErrTemp->data
12637 [localB->b_i_l];
12638 }
12639
12640 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12641 localB->x[localB->b_i_l] = 0.0;
12642 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
12643 localB->A_f = localB->unusedU0[6 * localB->idx +
12644 localB->b_i_l] * grad->data[localB->idx] + localB->
12645 x[localB->b_i_l];
12646 localB->x[localB->b_i_l] = localB->A_f;
12647 }
12648 }
12649
12650 *err = cartesian_trajectory_pla_norm_j(localB->x);
12651 *iter = static_cast<real_T>(localB->g_idx_0) + 1.0;
12652 exitg1 = 1;
12653 } else {
12654 localB->b_gamma *= localB->beta;
12655 localB->m++;
12656 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12657 localB->sNew_m[localB->b_i_l] = localB->b_gamma * localB->
12658 Hg[localB->b_i_l] + localB->x[localB->b_i_l];
12659 }
12660
12661 cartesian_IKHelpers_computeCost(localB->sNew_m, obj->ExtraArgs,
12662 &localB->costNew, localB->V, unusedU1, &d, localB);
12663 obj->ExtraArgs = d;
12664 }
12665 } else {
12666 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12667 xSol[localB->b_i_l] += localB->x[localB->b_i_l];
12668 }
12669
12670 args = obj->ExtraArgs;
12671 localB->b_i_l = alpha->size[0];
12672 alpha->size[0] = args->GradTemp->size[0];
12673 cartes_emxEnsureCapacity_real_T(alpha, localB->b_i_l, localB);
12674 localB->i_i = args->GradTemp->size[0];
12675 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i;
12676 localB->b_i_l++) {
12677 alpha->data[localB->b_i_l] = args->GradTemp->data
12678 [localB->b_i_l];
12679 }
12680
12681 exitg1 = 2;
12682 }
12683 } while (exitg1 == 0);
12684
12685 if (exitg1 == 1) {
12686 exitg2 = 1;
12687 } else if ((localB->m == 0.0) && (fabs(localB->b_gamma - localB->s_j)
12688 < 1.4901161193847656E-8)) {
12689 localB->i_i = obj->ConstraintMatrix->size[0];
12690 localB->b_i_l = grad->size[0];
12691 grad->size[0] = localB->i_i;
12692 cartes_emxEnsureCapacity_real_T(grad, localB->b_i_l, localB);
12693 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l
12694 ++) {
12695 grad->data[localB->b_i_l] = obj->ConstraintMatrix->data
12696 [(localB->idxl - 1) * obj->ConstraintMatrix->size[0] +
12697 localB->b_i_l];
12698 }
12699
12700 activeSet->data[localB->idxl - 1] = true;
12701 localB->nx_n = activeSet->size[0] - 1;
12702 localB->idx = 0;
12703 for (localB->b_i_l = 0; localB->b_i_l <= localB->nx_n;
12704 localB->b_i_l++) {
12705 if (activeSet->data[localB->b_i_l]) {
12706 localB->idx++;
12707 }
12708 }
12709
12710 localB->b_i_l = gb->size[0];
12711 gb->size[0] = localB->idx;
12712 carte_emxEnsureCapacity_int32_T(gb, localB->b_i_l);
12713 localB->idx = 0;
12714 for (localB->b_i_l = 0; localB->b_i_l <= localB->nx_n;
12715 localB->b_i_l++) {
12716 if (activeSet->data[localB->b_i_l]) {
12717 gb->data[localB->idx] = localB->b_i_l + 1;
12718 localB->idx++;
12719 }
12720 }
12721
12722 localB->i_i = obj->ConstraintMatrix->size[0];
12723 localB->b_i_l = A->size[0] * A->size[1];
12724 A->size[0] = localB->i_i;
12725 A->size[1] = gb->size[0];
12726 cartes_emxEnsureCapacity_real_T(A, localB->b_i_l, localB);
12727 localB->n = gb->size[0];
12728 for (localB->b_i_l = 0; localB->b_i_l < localB->n; localB->b_i_l++)
12729 {
12730 for (localB->idx = 0; localB->idx < localB->i_i; localB->idx++)
12731 {
12732 A->data[localB->idx + A->size[0] * localB->b_i_l] =
12733 obj->ConstraintMatrix->data[(gb->data[localB->b_i_l] - 1) *
12734 obj->ConstraintMatrix->size[0] + localB->idx];
12735 }
12736 }
12737
12738 localB->b_i_l = AIn->size[0] * AIn->size[1];
12739 AIn->size[0] = grad->size[0];
12740 AIn->size[1] = grad->size[0];
12741 cartes_emxEnsureCapacity_real_T(AIn, localB->b_i_l, localB);
12742 localB->i_i = grad->size[0];
12743 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l
12744 ++) {
12745 localB->n = grad->size[0];
12746 for (localB->idx = 0; localB->idx < localB->n; localB->idx++) {
12747 AIn->data[localB->idx + AIn->size[0] * localB->b_i_l] =
12748 grad->data[localB->idx] * grad->data[localB->b_i_l];
12749 }
12750 }
12751
12752 localB->m_i = AIn->size[0] - 1;
12753 localB->inner = AIn->size[1] - 1;
12754 localB->b_i_l = y_0->size[0] * y_0->size[1];
12755 y_0->size[0] = AIn->size[0];
12756 y_0->size[1] = 6;
12757 cartes_emxEnsureCapacity_real_T(y_0, localB->b_i_l, localB);
12758 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
12759 localB->coffset = (localB->m_i + 1) * localB->idx - 1;
12760 localB->boffset = localB->idx * 6 - 1;
12761 for (localB->b_i_l = 0; localB->b_i_l <= localB->m_i;
12762 localB->b_i_l++) {
12763 y_0->data[(localB->coffset + localB->b_i_l) + 1] = 0.0;
12764 }
12765
12766 for (localB->nx_n = 0; localB->nx_n <= localB->inner;
12767 localB->nx_n++) {
12768 localB->aoffset = localB->nx_n * AIn->size[0] - 1;
12769 localB->s_j = localB->H[(localB->boffset + localB->nx_n) + 1];
12770 for (localB->j = 0; localB->j <= localB->m_i; localB->j++) {
12771 localB->i_i = localB->j + 1;
12772 localB->b_i_l = localB->coffset + localB->i_i;
12773 y_0->data[localB->b_i_l] += AIn->data[localB->aoffset +
12774 localB->i_i] * localB->s_j;
12775 }
12776 }
12777 }
12778
12779 localB->b_i_l = grad_1->size[0] * grad_1->size[1];
12780 grad_1->size[0] = 1;
12781 grad_1->size[1] = grad->size[0];
12782 cartes_emxEnsureCapacity_real_T(grad_1, localB->b_i_l, localB);
12783 localB->i_i = grad->size[0];
12784 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l
12785 ++) {
12786 grad_1->data[localB->b_i_l] = grad->data[localB->b_i_l];
12787 }
12788
12789 localB->beta = 0.0;
12790 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12791 localB->sNew_m[localB->b_i_l] = 0.0;
12792 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
12793 localB->sigma = localB->H[6 * localB->b_i_l + localB->idx] *
12794 grad_1->data[localB->idx] + localB->sNew_m[localB->b_i_l];
12795 localB->sNew_m[localB->b_i_l] = localB->sigma;
12796 }
12797
12798 localB->beta += localB->sNew_m[localB->b_i_l] * grad->
12799 data[localB->b_i_l];
12800 }
12801
12802 localB->s_j = 1.0 / localB->beta;
12803 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12804 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
12805 localB->idxl = localB->b_i_l + 6 * localB->idx;
12806 localB->H_m[localB->idxl] = 0.0;
12807 for (localB->i_i = 0; localB->i_i < 6; localB->i_i++) {
12808 localB->H_m[localB->idxl] += localB->H[6 * localB->i_i +
12809 localB->b_i_l] * y_0->data[6 * localB->idx + localB->i_i];
12810 }
12811 }
12812 }
12813
12814 for (localB->b_i_l = 0; localB->b_i_l < 36; localB->b_i_l++) {
12815 localB->H[localB->b_i_l] -= localB->s_j * localB->H_m
12816 [localB->b_i_l];
12817 }
12818
12819 guard1 = true;
12820 } else {
12821 localB->b_i_l = grad->size[0];
12822 grad->size[0] = alpha->size[0];
12823 cartes_emxEnsureCapacity_real_T(grad, localB->b_i_l, localB);
12824 localB->i_i = alpha->size[0];
12825 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l
12826 ++) {
12827 grad->data[localB->b_i_l] = alpha->data[localB->b_i_l] -
12828 grad->data[localB->b_i_l];
12829 }
12830
12831 localB->b_gamma = 0.0;
12832 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12833 localB->b_gamma += localB->Hg[localB->b_i_l] * grad->data
12834 [localB->b_i_l];
12835 }
12836
12837 localB->b_i_l = tmp->size[0] * tmp->size[1];
12838 tmp->size[0] = 1;
12839 tmp->size[1] = grad->size[0];
12840 cartes_emxEnsureCapacity_real_T(tmp, localB->b_i_l, localB);
12841 localB->i_i = grad->size[0];
12842 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l
12843 ++) {
12844 tmp->data[localB->b_i_l] = 0.2 * grad->data[localB->b_i_l];
12845 }
12846
12847 localB->s_j = 0.0;
12848 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12849 localB->sNew_m[localB->b_i_l] = 0.0;
12850 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
12851 localB->beta = localB->H[6 * localB->b_i_l + localB->idx] *
12852 tmp->data[localB->idx] + localB->sNew_m[localB->b_i_l];
12853 localB->sNew_m[localB->b_i_l] = localB->beta;
12854 }
12855
12856 localB->s_j += localB->sNew_m[localB->b_i_l] * grad->data
12857 [localB->b_i_l];
12858 }
12859
12860 if (localB->b_gamma < localB->s_j) {
12861 localB->b_i_l = tmp_0->size[0] * tmp_0->size[1];
12862 tmp_0->size[0] = 1;
12863 tmp_0->size[1] = grad->size[0];
12864 cartes_emxEnsureCapacity_real_T(tmp_0, localB->b_i_l, localB);
12865 localB->i_i = grad->size[0];
12866 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i;
12867 localB->b_i_l++) {
12868 tmp_0->data[localB->b_i_l] = 0.8 * grad->data[localB->b_i_l];
12869 }
12870
12871 localB->s_j = 0.0;
12872 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12873 localB->sNew_m[localB->b_i_l] = 0.0;
12874 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
12875 localB->beta = localB->H[6 * localB->b_i_l + localB->idx] *
12876 tmp_0->data[localB->idx] + localB->sNew_m[localB->b_i_l];
12877 localB->sNew_m[localB->b_i_l] = localB->beta;
12878 }
12879
12880 localB->s_j += localB->sNew_m[localB->b_i_l] * grad->
12881 data[localB->b_i_l];
12882 }
12883
12884 localB->b_i_l = grad_0->size[0] * grad_0->size[1];
12885 grad_0->size[0] = 1;
12886 grad_0->size[1] = grad->size[0];
12887 cartes_emxEnsureCapacity_real_T(grad_0, localB->b_i_l, localB);
12888 localB->i_i = grad->size[0];
12889 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i;
12890 localB->b_i_l++) {
12891 grad_0->data[localB->b_i_l] = grad->data[localB->b_i_l];
12892 }
12893
12894 localB->beta = 0.0;
12895 localB->b_gamma = 0.0;
12896 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12897 localB->sNew_m[localB->b_i_l] = 0.0;
12898 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
12899 localB->sigma = localB->H[6 * localB->b_i_l + localB->idx] *
12900 grad_0->data[localB->idx] + localB->sNew_m[localB->b_i_l];
12901 localB->sNew_m[localB->b_i_l] = localB->sigma;
12902 }
12903
12904 localB->beta += localB->sNew_m[localB->b_i_l] * grad->
12905 data[localB->b_i_l];
12906 localB->b_gamma += localB->Hg[localB->b_i_l] * grad->
12907 data[localB->b_i_l];
12908 }
12909
12910 localB->b_gamma = localB->s_j / (localB->beta - localB->b_gamma);
12911 } else {
12912 localB->b_gamma = 1.0;
12913 }
12914
12915 localB->beta = 0.0;
12916 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12917 localB->s_j = 0.0;
12918 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
12919 localB->s_j += localB->H[6 * localB->idx + localB->b_i_l] *
12920 (1.0 - localB->b_gamma) * grad->data[localB->idx];
12921 }
12922
12923 localB->s_j += localB->b_gamma * localB->Hg[localB->b_i_l];
12924 localB->beta += localB->s_j * grad->data[localB->b_i_l];
12925 localB->sNew_m[localB->b_i_l] = localB->s_j;
12926 }
12927
12928 localB->b_i_l = sNew->size[0] * sNew->size[1];
12929 sNew->size[0] = 6;
12930 sNew->size[1] = grad->size[0];
12931 cartes_emxEnsureCapacity_real_T(sNew, localB->b_i_l, localB);
12932 localB->i_i = grad->size[0];
12933 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l
12934 ++) {
12935 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
12936 localB->s_j = localB->sNew_m[localB->idx] * grad->data
12937 [localB->b_i_l];
12938 sNew->data[localB->idx + 6 * localB->b_i_l] = localB->s_j /
12939 localB->beta;
12940 }
12941 }
12942
12943 for (localB->b_i_l = 0; localB->b_i_l < 36; localB->b_i_l++) {
12944 localB->V[localB->b_i_l] = localB->unusedU0[localB->b_i_l] -
12945 sNew->data[localB->b_i_l];
12946 }
12947
12948 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12949 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
12950 localB->nx_n = localB->b_i_l + 6 * localB->idx;
12951 localB->H_m[localB->nx_n] = 0.0;
12952 for (localB->i_i = 0; localB->i_i < 6; localB->i_i++) {
12953 localB->H_m[localB->nx_n] += localB->V[6 * localB->i_i +
12954 localB->b_i_l] * localB->H[6 * localB->idx + localB->i_i];
12955 }
12956 }
12957 }
12958
12959 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12960 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
12961 localB->nx_n = localB->b_i_l + 6 * localB->idx;
12962 localB->P[localB->nx_n] = 0.0;
12963 for (localB->i_i = 0; localB->i_i < 6; localB->i_i++) {
12964 localB->P[localB->nx_n] += localB->H_m[6 * localB->i_i +
12965 localB->b_i_l] * localB->V[6 * localB->i_i + localB->idx];
12966 }
12967 }
12968 }
12969
12970 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
12971 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
12972 localB->sNew[localB->idx + 6 * localB->b_i_l] = localB->
12973 sNew_m[localB->idx] * localB->sNew_m[localB->b_i_l] /
12974 localB->beta;
12975 }
12976 }
12977
12978 for (localB->b_i_l = 0; localB->b_i_l < 36; localB->b_i_l++) {
12979 localB->s_j = localB->P[localB->b_i_l] + localB->sNew
12980 [localB->b_i_l];
12981 localB->H_m[localB->b_i_l] = 1.4901161193847656E-8 *
12982 static_cast<real_T>(tmp_1[localB->b_i_l]) + localB->s_j;
12983 localB->H[localB->b_i_l] = localB->s_j;
12984 }
12985
12986 if (!cartesian_tr_isPositiveDefinite(localB->H_m, localB)) {
12987 *exitFlag = HessianNotPositiveSemidefinite;
12988 args = obj->ExtraArgs;
12989 for (localB->b_i_l = 0; localB->b_i_l < 36; localB->b_i_l++) {
12990 localB->unusedU0[localB->b_i_l] = args->WeightMatrix
12991 [localB->b_i_l];
12992 }
12993
12994 localB->b_i_l = grad->size[0];
12995 grad->size[0] = args->ErrTemp->size[0];
12996 cartes_emxEnsureCapacity_real_T(grad, localB->b_i_l, localB);
12997 localB->i_i = args->ErrTemp->size[0];
12998 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i;
12999 localB->b_i_l++) {
13000 grad->data[localB->b_i_l] = args->ErrTemp->data[localB->b_i_l];
13001 }
13002
13003 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
13004 localB->x[localB->b_i_l] = 0.0;
13005 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
13006 localB->A_f = localB->unusedU0[6 * localB->idx +
13007 localB->b_i_l] * grad->data[localB->idx] + localB->
13008 x[localB->b_i_l];
13009 localB->x[localB->b_i_l] = localB->A_f;
13010 }
13011 }
13012
13013 *err = cartesian_trajectory_pla_norm_j(localB->x);
13014 *iter = static_cast<real_T>(localB->g_idx_0) + 1.0;
13015 exitg2 = 1;
13016 } else {
13017 guard1 = true;
13018 }
13019 }
13020 }
13021
13022 if (guard1) {
13023 if (DampedBFGSwGradientProjection_k(obj, xSol, localB)) {
13024 for (localB->i_i = 0; localB->i_i < 6; localB->i_i++) {
13025 xSol[localB->i_i] = localB->x[localB->i_i];
13026 }
13027
13028 *exitFlag = SearchDirectionInvalid;
13029 args = obj->ExtraArgs;
13030 for (localB->b_i_l = 0; localB->b_i_l < 36; localB->b_i_l++) {
13031 localB->unusedU0[localB->b_i_l] = args->WeightMatrix
13032 [localB->b_i_l];
13033 }
13034
13035 localB->b_i_l = grad->size[0];
13036 grad->size[0] = args->ErrTemp->size[0];
13037 cartes_emxEnsureCapacity_real_T(grad, localB->b_i_l, localB);
13038 localB->i_i = args->ErrTemp->size[0];
13039 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l
13040 ++) {
13041 grad->data[localB->b_i_l] = args->ErrTemp->data[localB->b_i_l];
13042 }
13043
13044 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
13045 localB->x[localB->b_i_l] = 0.0;
13046 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
13047 localB->A_f = localB->unusedU0[6 * localB->idx + localB->b_i_l]
13048 * grad->data[localB->idx] + localB->x[localB->b_i_l];
13049 localB->x[localB->b_i_l] = localB->A_f;
13050 }
13051 }
13052
13053 *err = cartesian_trajectory_pla_norm_j(localB->x);
13054 *iter = static_cast<real_T>(localB->g_idx_0) + 1.0;
13055 exitg2 = 1;
13056 } else {
13057 for (localB->i_i = 0; localB->i_i < 6; localB->i_i++) {
13058 localB->x[localB->i_i] = xSol[localB->i_i];
13059 }
13060
13061 localB->b_i_l = grad->size[0];
13062 grad->size[0] = alpha->size[0];
13063 cartes_emxEnsureCapacity_real_T(grad, localB->b_i_l, localB);
13064 localB->i_i = alpha->size[0];
13065 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l
13066 ++) {
13067 grad->data[localB->b_i_l] = alpha->data[localB->b_i_l];
13068 }
13069
13070 localB->cost = localB->costNew;
13071 localB->g_idx_0++;
13072 }
13073 }
13074 }
13075 }
13076 } else {
13077 *exitFlag = IterationLimitExceeded;
13078 args = obj->ExtraArgs;
13079 for (localB->b_i_l = 0; localB->b_i_l < 36; localB->b_i_l++) {
13080 localB->unusedU0[localB->b_i_l] = args->WeightMatrix[localB->b_i_l];
13081 }
13082
13083 localB->b_i_l = grad->size[0];
13084 grad->size[0] = args->ErrTemp->size[0];
13085 cartes_emxEnsureCapacity_real_T(grad, localB->b_i_l, localB);
13086 localB->i_i = args->ErrTemp->size[0];
13087 for (localB->b_i_l = 0; localB->b_i_l < localB->i_i; localB->b_i_l++) {
13088 grad->data[localB->b_i_l] = args->ErrTemp->data[localB->b_i_l];
13089 }
13090
13091 for (localB->b_i_l = 0; localB->b_i_l < 6; localB->b_i_l++) {
13092 localB->x[localB->b_i_l] = 0.0;
13093 for (localB->idx = 0; localB->idx < 6; localB->idx++) {
13094 localB->A_f = localB->unusedU0[6 * localB->idx + localB->b_i_l] *
13095 grad->data[localB->idx] + localB->x[localB->b_i_l];
13096 localB->x[localB->b_i_l] = localB->A_f;
13097 }
13098 }
13099
13100 *err = cartesian_trajectory_pla_norm_j(localB->x);
13101 *iter = obj->MaxNumIterationInternal;
13102 exitg2 = 1;
13103 }
13104 } while (exitg2 == 0);
13105
13106 cartesian_traje_emxFree_int32_T(&ii_3);
13107 cartesian_trajec_emxFree_real_T(&alpha_0);
13108 cartesian_trajec_emxFree_real_T(&A_3);
13109 cartesian_trajec_emxFree_real_T(&grad_1);
13110 cartesian_traje_emxFree_int32_T(&ii_2);
13111 cartesian_traje_emxFree_int32_T(&ii_1);
13112 cartesian_trajec_emxFree_real_T(&sNew);
13113 cartesian_trajec_emxFree_real_T(&grad_0);
13114 cartesian_trajec_emxFree_real_T(&tmp_0);
13115 cartesian_trajec_emxFree_real_T(&tmp);
13116 cartesian_trajec_emxFree_real_T(&sigma);
13117 cartesian_trajec_emxFree_real_T(&A_2);
13118 cartesian_tra_emxFree_boolean_T(&x);
13119 cartesian_trajec_emxFree_real_T(&y_0);
13120 cartesian_traje_emxFree_int32_T(&ii_0);
13121 cartesian_trajec_emxFree_real_T(&y);
13122 cartesian_traje_emxFree_int32_T(&ii);
13123 cartesian_trajec_emxFree_real_T(&a);
13124 cartesian_traje_emxFree_int32_T(&gb);
13125 cartesian_traje_emxFree_int32_T(&fb);
13126 cartesian_traje_emxFree_int32_T(&eb);
13127 cartesian_traje_emxFree_int32_T(&db);
13128 cartesian_traje_emxFree_int32_T(&cb);
13129 cartesian_trajec_emxFree_real_T(&L);
13130 cartesian_trajec_emxFree_real_T(&AIn);
13131 cartesian_trajec_emxFree_real_T(&alpha);
13132 cartesian_trajec_emxFree_real_T(&A);
13133 cartesian_tra_emxFree_boolean_T(&activeSet);
13134 cartesian_trajec_emxFree_real_T(&grad);
13135 cartesian_trajec_emxFree_real_T(&unusedU1);
13136}
13137
13138static void cartesian_trajectory_p_isfinite(const
13139 emxArray_real_T_cartesian_tra_T *x, emxArray_boolean_T_cartesian__T *b)
13140{
13141 emxArray_boolean_T_cartesian__T *tmp;
13142 int32_T loop_ub;
13143 int32_T i;
13144 i = b->size[0];
13145 b->size[0] = x->size[0];
13146 car_emxEnsureCapacity_boolean_T(b, i);
13147 loop_ub = x->size[0];
13148 for (i = 0; i < loop_ub; i++) {
13149 b->data[i] = rtIsInf(x->data[i]);
13150 }
13151
13152 cartesian_tra_emxInit_boolean_T(&tmp, 1);
13153 i = tmp->size[0];
13154 tmp->size[0] = x->size[0];
13155 car_emxEnsureCapacity_boolean_T(tmp, i);
13156 loop_ub = x->size[0];
13157 for (i = 0; i < loop_ub; i++) {
13158 tmp->data[i] = rtIsNaN(x->data[i]);
13159 }
13160
13161 i = b->size[0];
13162 car_emxEnsureCapacity_boolean_T(b, i);
13163 loop_ub = b->size[0];
13164 for (i = 0; i < loop_ub; i++) {
13165 b->data[i] = ((!b->data[i]) && (!tmp->data[i]));
13166 }
13167
13168 cartesian_tra_emxFree_boolean_T(&tmp);
13169}
13170
13171static void cartesian_trajectory_pla_rand_m(real_T varargin_1,
13172 emxArray_real_T_cartesian_tra_T *r, B_MATLABSystem_cartesian_traj_T *localB,
13173 DW_MATLABSystem_cartesian_tra_T *localDW)
13174{
13175 localB->b_k_f = r->size[0];
13176 localB->d_p = static_cast<int32_T>(varargin_1);
13177 r->size[0] = localB->d_p;
13178 cartes_emxEnsureCapacity_real_T(r, localB->b_k_f, localB);
13179 localB->d_p--;
13180 for (localB->b_k_f = 0; localB->b_k_f <= localB->d_p; localB->b_k_f++) {
13181 memcpy(&localB->uv1[0], &localDW->state_e[0], 625U * sizeof(uint32_T));
13182 cartesian__eml_rand_mt19937ar_a(localB->uv1, localDW->state_e, &r->
13183 data[localB->b_k_f], localB);
13184 }
13185}
13186
13187static real_T cartesian_trajectory_genrandu_n(uint32_T mt[625],
13188 B_MATLABSystem_cartesian_traj_T *localB)
13189{
13190 real_T r;
13191 int32_T exitg1;
13192
13193 // ========================= COPYRIGHT NOTICE ============================
13194 // This is a uniform (0,1) pseudorandom number generator based on:
13195 //
13196 // A C-program for MT19937, with initialization improved 2002/1/26.
13197 // Coded by Takuji Nishimura and Makoto Matsumoto.
13198 //
13199 // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
13200 // All rights reserved.
13201 //
13202 // Redistribution and use in source and binary forms, with or without
13203 // modification, are permitted provided that the following conditions
13204 // are met:
13205 //
13206 // 1. Redistributions of source code must retain the above copyright
13207 // notice, this list of conditions and the following disclaimer.
13208 //
13209 // 2. Redistributions in binary form must reproduce the above copyright
13210 // notice, this list of conditions and the following disclaimer
13211 // in the documentation and/or other materials provided with the
13212 // distribution.
13213 //
13214 // 3. The names of its contributors may not be used to endorse or
13215 // promote products derived from this software without specific
13216 // prior written permission.
13217 //
13218 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
13219 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
13220 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
13221 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
13222 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
13223 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
13224 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
13225 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
13226 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
13227 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
13228 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
13229 //
13230 // ============================= END =================================
13231 do {
13232 exitg1 = 0;
13233 cartesi_genrand_uint32_vector_d(mt, localB->b_u_o, localB);
13234 r = (static_cast<real_T>(localB->b_u_o[0] >> 5U) * 6.7108864E+7 +
13235 static_cast<real_T>(localB->b_u_o[1] >> 6U)) * 1.1102230246251565E-16;
13236 if (r == 0.0) {
13237 if (!cartesian_trajec_is_valid_state(mt, localB)) {
13238 localB->r_k = 5489U;
13239 mt[0] = 5489U;
13240 for (localB->b_mti_i = 0; localB->b_mti_i < 623; localB->b_mti_i++) {
13241 localB->r_k = ((localB->r_k >> 30U ^ localB->r_k) * 1812433253U +
13242 localB->b_mti_i) + 1U;
13243 mt[localB->b_mti_i + 1] = localB->r_k;
13244 }
13245
13246 mt[624] = 624U;
13247 }
13248 } else {
13249 exitg1 = 1;
13250 }
13251 } while (exitg1 == 0);
13252
13253 return r;
13254}
13255
13256static real_T cartesia_eml_rand_mt19937ar_aoi(uint32_T state[625],
13257 B_MATLABSystem_cartesian_traj_T *localB)
13258{
13259 real_T r;
13260 static const real_T tmp[257] = { 1.0, 0.977101701267673, 0.959879091800108,
13261 0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,
13262 0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,
13263 0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,
13264 0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,
13265 0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,
13266 0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,
13267 0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,
13268 0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,
13269 0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,
13270 0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,
13271 0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,
13272 0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,
13273 0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,
13274 0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,
13275 0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,
13276 0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,
13277 0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,
13278 0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,
13279 0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,
13280 0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,
13281 0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,
13282 0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,
13283 0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,
13284 0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,
13285 0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,
13286 0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,
13287 0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,
13288 0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,
13289 0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,
13290 0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,
13291 0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,
13292 0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,
13293 0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,
13294 0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,
13295 0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,
13296 0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,
13297 0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,
13298 0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,
13299 0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,
13300 0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,
13301 0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,
13302 0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,
13303 0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,
13304 0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,
13305 0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,
13306 0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,
13307 0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,
13308 0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,
13309 0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,
13310 0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,
13311 0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,
13312 0.093365311912691, 0.0911136480663738, 0.0888735920682759,
13313 0.0866451944505581, 0.0844285095703535, 0.082223595813203,
13314 0.0800305158146631, 0.0778493367020961, 0.0756801303589272,
13315 0.0735229737139814, 0.0713779490588905, 0.0692451443970068,
13316 0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,
13317 0.0587679529209339, 0.0567106901062031, 0.0546664613248891,
13318 0.0526354182767924, 0.0506177238609479, 0.0486135532158687,
13319 0.0466230949019305, 0.0446465522512946, 0.0426841449164746,
13320 0.0407361106559411, 0.0388027074045262, 0.0368842156885674,
13321 0.0349809414617162, 0.0330932194585786, 0.0312214171919203,
13322 0.0293659397581334, 0.0275272356696031, 0.0257058040085489,
13323 0.0239022033057959, 0.0221170627073089, 0.0203510962300445,
13324 0.0186051212757247, 0.0168800831525432, 0.0151770883079353,
13325 0.0134974506017399, 0.0118427578579079, 0.0102149714397015,
13326 0.00861658276939875, 0.00705087547137324, 0.00552240329925101,
13327 0.00403797259336304, 0.00260907274610216, 0.0012602859304986,
13328 0.000477467764609386 };
13329
13330 const real_T *fitab;
13331 int32_T exitg1;
13332 localB->xi[0] = 0.0;
13333 localB->xi[1] = 0.215241895984875;
13334 localB->xi[2] = 0.286174591792068;
13335 localB->xi[3] = 0.335737519214422;
13336 localB->xi[4] = 0.375121332878378;
13337 localB->xi[5] = 0.408389134611989;
13338 localB->xi[6] = 0.43751840220787;
13339 localB->xi[7] = 0.46363433679088;
13340 localB->xi[8] = 0.487443966139235;
13341 localB->xi[9] = 0.50942332960209;
13342 localB->xi[10] = 0.529909720661557;
13343 localB->xi[11] = 0.549151702327164;
13344 localB->xi[12] = 0.567338257053817;
13345 localB->xi[13] = 0.584616766106378;
13346 localB->xi[14] = 0.601104617755991;
13347 localB->xi[15] = 0.61689699000775;
13348 localB->xi[16] = 0.63207223638606;
13349 localB->xi[17] = 0.646695714894993;
13350 localB->xi[18] = 0.660822574244419;
13351 localB->xi[19] = 0.674499822837293;
13352 localB->xi[20] = 0.687767892795788;
13353 localB->xi[21] = 0.700661841106814;
13354 localB->xi[22] = 0.713212285190975;
13355 localB->xi[23] = 0.725446140909999;
13356 localB->xi[24] = 0.737387211434295;
13357 localB->xi[25] = 0.749056662017815;
13358 localB->xi[26] = 0.760473406430107;
13359 localB->xi[27] = 0.771654424224568;
13360 localB->xi[28] = 0.782615023307232;
13361 localB->xi[29] = 0.793369058840623;
13362 localB->xi[30] = 0.80392911698997;
13363 localB->xi[31] = 0.814306670135215;
13364 localB->xi[32] = 0.824512208752291;
13365 localB->xi[33] = 0.834555354086381;
13366 localB->xi[34] = 0.844444954909153;
13367 localB->xi[35] = 0.854189171008163;
13368 localB->xi[36] = 0.863795545553308;
13369 localB->xi[37] = 0.87327106808886;
13370 localB->xi[38] = 0.882622229585165;
13371 localB->xi[39] = 0.891855070732941;
13372 localB->xi[40] = 0.900975224461221;
13373 localB->xi[41] = 0.909987953496718;
13374 localB->xi[42] = 0.91889818364959;
13375 localB->xi[43] = 0.927710533401999;
13376 localB->xi[44] = 0.936429340286575;
13377 localB->xi[45] = 0.945058684468165;
13378 localB->xi[46] = 0.953602409881086;
13379 localB->xi[47] = 0.96206414322304;
13380 localB->xi[48] = 0.970447311064224;
13381 localB->xi[49] = 0.978755155294224;
13382 localB->xi[50] = 0.986990747099062;
13383 localB->xi[51] = 0.99515699963509;
13384 localB->xi[52] = 1.00325667954467;
13385 localB->xi[53] = 1.01129241744;
13386 localB->xi[54] = 1.01926671746548;
13387 localB->xi[55] = 1.02718196603564;
13388 localB->xi[56] = 1.03504043983344;
13389 localB->xi[57] = 1.04284431314415;
13390 localB->xi[58] = 1.05059566459093;
13391 localB->xi[59] = 1.05829648333067;
13392 localB->xi[60] = 1.06594867476212;
13393 localB->xi[61] = 1.07355406579244;
13394 localB->xi[62] = 1.0811144097034;
13395 localB->xi[63] = 1.08863139065398;
13396 localB->xi[64] = 1.09610662785202;
13397 localB->xi[65] = 1.10354167942464;
13398 localB->xi[66] = 1.11093804601357;
13399 localB->xi[67] = 1.11829717411934;
13400 localB->xi[68] = 1.12562045921553;
13401 localB->xi[69] = 1.13290924865253;
13402 localB->xi[70] = 1.14016484436815;
13403 localB->xi[71] = 1.14738850542085;
13404 localB->xi[72] = 1.15458145035993;
13405 localB->xi[73] = 1.16174485944561;
13406 localB->xi[74] = 1.16887987673083;
13407 localB->xi[75] = 1.17598761201545;
13408 localB->xi[76] = 1.18306914268269;
13409 localB->xi[77] = 1.19012551542669;
13410 localB->xi[78] = 1.19715774787944;
13411 localB->xi[79] = 1.20416683014438;
13412 localB->xi[80] = 1.2111537262437;
13413 localB->xi[81] = 1.21811937548548;
13414 localB->xi[82] = 1.22506469375653;
13415 localB->xi[83] = 1.23199057474614;
13416 localB->xi[84] = 1.23889789110569;
13417 localB->xi[85] = 1.24578749554863;
13418 localB->xi[86] = 1.2526602218949;
13419 localB->xi[87] = 1.25951688606371;
13420 localB->xi[88] = 1.26635828701823;
13421 localB->xi[89] = 1.27318520766536;
13422 localB->xi[90] = 1.27999841571382;
13423 localB->xi[91] = 1.28679866449324;
13424 localB->xi[92] = 1.29358669373695;
13425 localB->xi[93] = 1.30036323033084;
13426 localB->xi[94] = 1.30712898903073;
13427 localB->xi[95] = 1.31388467315022;
13428 localB->xi[96] = 1.32063097522106;
13429 localB->xi[97] = 1.32736857762793;
13430 localB->xi[98] = 1.33409815321936;
13431 localB->xi[99] = 1.3408203658964;
13432 localB->xi[100] = 1.34753587118059;
13433 localB->xi[101] = 1.35424531676263;
13434 localB->xi[102] = 1.36094934303328;
13435 localB->xi[103] = 1.36764858359748;
13436 localB->xi[104] = 1.37434366577317;
13437 localB->xi[105] = 1.38103521107586;
13438 localB->xi[106] = 1.38772383568998;
13439 localB->xi[107] = 1.39441015092814;
13440 localB->xi[108] = 1.40109476367925;
13441 localB->xi[109] = 1.4077782768464;
13442 localB->xi[110] = 1.41446128977547;
13443 localB->xi[111] = 1.42114439867531;
13444 localB->xi[112] = 1.42782819703026;
13445 localB->xi[113] = 1.43451327600589;
13446 localB->xi[114] = 1.44120022484872;
13447 localB->xi[115] = 1.44788963128058;
13448 localB->xi[116] = 1.45458208188841;
13449 localB->xi[117] = 1.46127816251028;
13450 localB->xi[118] = 1.46797845861808;
13451 localB->xi[119] = 1.47468355569786;
13452 localB->xi[120] = 1.48139403962819;
13453 localB->xi[121] = 1.48811049705745;
13454 localB->xi[122] = 1.49483351578049;
13455 localB->xi[123] = 1.50156368511546;
13456 localB->xi[124] = 1.50830159628131;
13457 localB->xi[125] = 1.51504784277671;
13458 localB->xi[126] = 1.521803020761;
13459 localB->xi[127] = 1.52856772943771;
13460 localB->xi[128] = 1.53534257144151;
13461 localB->xi[129] = 1.542128153229;
13462 localB->xi[130] = 1.54892508547417;
13463 localB->xi[131] = 1.55573398346918;
13464 localB->xi[132] = 1.56255546753104;
13465 localB->xi[133] = 1.56939016341512;
13466 localB->xi[134] = 1.57623870273591;
13467 localB->xi[135] = 1.58310172339603;
13468 localB->xi[136] = 1.58997987002419;
13469 localB->xi[137] = 1.59687379442279;
13470 localB->xi[138] = 1.60378415602609;
13471 localB->xi[139] = 1.61071162236983;
13472 localB->xi[140] = 1.61765686957301;
13473 localB->xi[141] = 1.62462058283303;
13474 localB->xi[142] = 1.63160345693487;
13475 localB->xi[143] = 1.63860619677555;
13476 localB->xi[144] = 1.64562951790478;
13477 localB->xi[145] = 1.65267414708306;
13478 localB->xi[146] = 1.65974082285818;
13479 localB->xi[147] = 1.66683029616166;
13480 localB->xi[148] = 1.67394333092612;
13481 localB->xi[149] = 1.68108070472517;
13482 localB->xi[150] = 1.68824320943719;
13483 localB->xi[151] = 1.69543165193456;
13484 localB->xi[152] = 1.70264685479992;
13485 localB->xi[153] = 1.7098896570713;
13486 localB->xi[154] = 1.71716091501782;
13487 localB->xi[155] = 1.72446150294804;
13488 localB->xi[156] = 1.73179231405296;
13489 localB->xi[157] = 1.73915426128591;
13490 localB->xi[158] = 1.74654827828172;
13491 localB->xi[159] = 1.75397532031767;
13492 localB->xi[160] = 1.76143636531891;
13493 localB->xi[161] = 1.76893241491127;
13494 localB->xi[162] = 1.77646449552452;
13495 localB->xi[163] = 1.78403365954944;
13496 localB->xi[164] = 1.79164098655216;
13497 localB->xi[165] = 1.79928758454972;
13498 localB->xi[166] = 1.80697459135082;
13499 localB->xi[167] = 1.81470317596628;
13500 localB->xi[168] = 1.82247454009388;
13501 localB->xi[169] = 1.83028991968276;
13502 localB->xi[170] = 1.83815058658281;
13503 localB->xi[171] = 1.84605785028518;
13504 localB->xi[172] = 1.8540130597602;
13505 localB->xi[173] = 1.86201760539967;
13506 localB->xi[174] = 1.87007292107127;
13507 localB->xi[175] = 1.878180486293;
13508 localB->xi[176] = 1.88634182853678;
13509 localB->xi[177] = 1.8945585256707;
13510 localB->xi[178] = 1.90283220855043;
13511 localB->xi[179] = 1.91116456377125;
13512 localB->xi[180] = 1.91955733659319;
13513 localB->xi[181] = 1.92801233405266;
13514 localB->xi[182] = 1.93653142827569;
13515 localB->xi[183] = 1.94511656000868;
13516 localB->xi[184] = 1.95376974238465;
13517 localB->xi[185] = 1.96249306494436;
13518 localB->xi[186] = 1.97128869793366;
13519 localB->xi[187] = 1.98015889690048;
13520 localB->xi[188] = 1.98910600761744;
13521 localB->xi[189] = 1.99813247135842;
13522 localB->xi[190] = 2.00724083056053;
13523 localB->xi[191] = 2.0164337349062;
13524 localB->xi[192] = 2.02571394786385;
13525 localB->xi[193] = 2.03508435372962;
13526 localB->xi[194] = 2.04454796521753;
13527 localB->xi[195] = 2.05410793165065;
13528 localB->xi[196] = 2.06376754781173;
13529 localB->xi[197] = 2.07353026351874;
13530 localB->xi[198] = 2.0833996939983;
13531 localB->xi[199] = 2.09337963113879;
13532 localB->xi[200] = 2.10347405571488;
13533 localB->xi[201] = 2.11368715068665;
13534 localB->xi[202] = 2.12402331568952;
13535 localB->xi[203] = 2.13448718284602;
13536 localB->xi[204] = 2.14508363404789;
13537 localB->xi[205] = 2.15581781987674;
13538 localB->xi[206] = 2.16669518035431;
13539 localB->xi[207] = 2.17772146774029;
13540 localB->xi[208] = 2.18890277162636;
13541 localB->xi[209] = 2.20024554661128;
13542 localB->xi[210] = 2.21175664288416;
13543 localB->xi[211] = 2.22344334009251;
13544 localB->xi[212] = 2.23531338492992;
13545 localB->xi[213] = 2.24737503294739;
13546 localB->xi[214] = 2.25963709517379;
13547 localB->xi[215] = 2.27210899022838;
13548 localB->xi[216] = 2.28480080272449;
13549 localB->xi[217] = 2.29772334890286;
13550 localB->xi[218] = 2.31088825060137;
13551 localB->xi[219] = 2.32430801887113;
13552 localB->xi[220] = 2.33799614879653;
13553 localB->xi[221] = 2.35196722737914;
13554 localB->xi[222] = 2.36623705671729;
13555 localB->xi[223] = 2.38082279517208;
13556 localB->xi[224] = 2.39574311978193;
13557 localB->xi[225] = 2.41101841390112;
13558 localB->xi[226] = 2.42667098493715;
13559 localB->xi[227] = 2.44272531820036;
13560 localB->xi[228] = 2.4592083743347;
13561 localB->xi[229] = 2.47614993967052;
13562 localB->xi[230] = 2.49358304127105;
13563 localB->xi[231] = 2.51154444162669;
13564 localB->xi[232] = 2.53007523215985;
13565 localB->xi[233] = 2.54922155032478;
13566 localB->xi[234] = 2.56903545268184;
13567 localB->xi[235] = 2.58957598670829;
13568 localB->xi[236] = 2.61091051848882;
13569 localB->xi[237] = 2.63311639363158;
13570 localB->xi[238] = 2.65628303757674;
13571 localB->xi[239] = 2.68051464328574;
13572 localB->xi[240] = 2.70593365612306;
13573 localB->xi[241] = 2.73268535904401;
13574 localB->xi[242] = 2.76094400527999;
13575 localB->xi[243] = 2.79092117400193;
13576 localB->xi[244] = 2.82287739682644;
13577 localB->xi[245] = 2.85713873087322;
13578 localB->xi[246] = 2.89412105361341;
13579 localB->xi[247] = 2.93436686720889;
13580 localB->xi[248] = 2.97860327988184;
13581 localB->xi[249] = 3.02783779176959;
13582 localB->xi[250] = 3.08352613200214;
13583 localB->xi[251] = 3.147889289518;
13584 localB->xi[252] = 3.2245750520478;
13585 localB->xi[253] = 3.32024473383983;
13586 localB->xi[254] = 3.44927829856143;
13587 localB->xi[255] = 3.65415288536101;
13588 localB->xi[256] = 3.91075795952492;
13589 fitab = &tmp[0];
13590 do {
13591 exitg1 = 0;
13592 cartesi_genrand_uint32_vector_d(state, localB->u32, localB);
13593 localB->i_fn = static_cast<int32_T>((localB->u32[1] >> 24U) + 1U);
13594 r = ((static_cast<real_T>(localB->u32[0] >> 3U) * 1.6777216E+7 +
13595 static_cast<real_T>(static_cast<int32_T>(localB->u32[1]) & 16777215)) *
13596 2.2204460492503131E-16 - 1.0) * localB->xi[localB->i_fn];
13597 if (fabs(r) <= localB->xi[localB->i_fn - 1]) {
13598 exitg1 = 1;
13599 } else if (localB->i_fn < 256) {
13600 localB->x_j = cartesian_trajectory_genrandu_n(state, localB);
13601 if ((fitab[localB->i_fn - 1] - fitab[localB->i_fn]) * localB->x_j +
13602 fitab[localB->i_fn] < exp(-0.5 * r * r)) {
13603 exitg1 = 1;
13604 }
13605 } else {
13606 do {
13607 localB->x_j = cartesian_trajectory_genrandu_n(state, localB);
13608 localB->x_j = log(localB->x_j) * 0.273661237329758;
13609 localB->d_u = cartesian_trajectory_genrandu_n(state, localB);
13610 } while (!(-2.0 * log(localB->d_u) > localB->x_j * localB->x_j));
13611
13612 if (r < 0.0) {
13613 r = localB->x_j - 3.65415288536101;
13614 } else {
13615 r = 3.65415288536101 - localB->x_j;
13616 }
13617
13618 exitg1 = 1;
13619 }
13620 } while (exitg1 == 0);
13621
13622 return r;
13623}
13624
13625static void cartesian_trajectory_plan_randn(const real_T varargin_1[2],
13626 emxArray_real_T_cartesian_tra_T *r, B_MATLABSystem_cartesian_traj_T *localB,
13627 DW_MATLABSystem_cartesian_tra_T *localDW)
13628{
13629 localB->b_k_hp = r->size[0] * r->size[1];
13630 r->size[0] = static_cast<int32_T>(varargin_1[0]);
13631 r->size[1] = 1;
13632 cartes_emxEnsureCapacity_real_T(r, localB->b_k_hp, localB);
13633 localB->d_h = r->size[0] - 1;
13634 for (localB->b_k_hp = 0; localB->b_k_hp <= localB->d_h; localB->b_k_hp++) {
13635 r->data[localB->b_k_hp] = cartesia_eml_rand_mt19937ar_aoi(localDW->state_e,
13636 localB);
13637 }
13638}
13639
13640static void cartes_NLPSolverInterface_solve(h_robotics_core_internal_Damp_T *obj,
13641 const real_T seed[6], real_T xSol[6], real_T *solutionInfo_Iterations, real_T *
13642 solutionInfo_RRAttempts, real_T *solutionInfo_Error, real_T
13643 *solutionInfo_ExitFlag, char_T solutionInfo_Status_data[], int32_T
13644 solutionInfo_Status_size[2], B_MATLABSystem_cartesian_traj_T *localB,
13645 DW_MATLABSystem_cartesian_tra_T *localDW)
13646{
13647 emxArray_real_T_cartesian_tra_T *newseed;
13648 f_robotics_manip_internal_IKE_T *args;
13649 x_robotics_manip_internal_Rig_T *obj_0;
13650 emxArray_real_T_cartesian_tra_T *qi;
13651 c_rigidBodyJoint_cartesian_tr_T *obj_1;
13652 emxArray_real_T_cartesian_tra_T *ub;
13653 emxArray_real_T_cartesian_tra_T *lb;
13654 emxArray_real_T_cartesian_tra_T *rn;
13655 emxArray_real_T_cartesian_tra_T *e;
13656 emxArray_boolean_T_cartesian__T *x;
13657 emxArray_boolean_T_cartesian__T *x_tmp;
13658 emxArray_boolean_T_cartesian__T *x_tmp_0;
13659 emxArray_boolean_T_cartesian__T *x_0;
13660 static const char_T tmp[14] = { 'b', 'e', 's', 't', ' ', 'a', 'v', 'a', 'i',
13661 'l', 'a', 'b', 'l', 'e' };
13662
13663 static const char_T tmp_0[7] = { 's', 'u', 'c', 'c', 'e', 's', 's' };
13664
13665 boolean_T guard1 = false;
13666 boolean_T guard2 = false;
13667 boolean_T guard3 = false;
13668 boolean_T exitg1;
13669 boolean_T exitg2;
13670 obj->MaxNumIterationInternal = obj->MaxNumIteration;
13671 obj->MaxTimeInternal = obj->MaxTime;
13672 for (localB->i_fq = 0; localB->i_fq < 6; localB->i_fq++) {
13673 obj->SeedInternal[localB->i_fq] = seed[localB->i_fq];
13674 }
13675
13676 localB->tol = obj->SolutionTolerance;
13677 obj->TimeObj.StartTime = ctimefun();
13678 DampedBFGSwGradientProjection_s(obj, xSol, &localB->exitFlag, &localB->err,
13679 &localB->iter, localB);
13680 *solutionInfo_RRAttempts = 0.0;
13681 *solutionInfo_Iterations = localB->iter;
13682 *solutionInfo_Error = localB->err;
13683 localB->exitFlagPrev = localB->exitFlag;
13684 cartesian_trajec_emxInit_real_T(&newseed, 1, localB);
13685 cartesian_trajec_emxInit_real_T(&qi, 2, localB);
13686 cartesian_trajec_emxInit_real_T(&ub, 1, localB);
13687 cartesian_trajec_emxInit_real_T(&lb, 1, localB);
13688 cartesian_trajec_emxInit_real_T(&rn, 1, localB);
13689 cartesian_trajec_emxInit_real_T(&e, 2, localB);
13690 cartesian_tra_emxInit_boolean_T(&x, 1);
13691 cartesian_tra_emxInit_boolean_T(&x_tmp, 1);
13692 cartesian_tra_emxInit_boolean_T(&x_tmp_0, 1);
13693 cartesian_tra_emxInit_boolean_T(&x_0, 1);
13694 exitg1 = false;
13695 while ((!exitg1) && (obj->RandomRestart && (localB->err > localB->tol))) {
13696 obj->MaxNumIterationInternal -= localB->iter;
13697 localB->err = ctimefun();
13698 localB->err -= obj->TimeObj.StartTime;
13699 obj->MaxTimeInternal = obj->MaxTime - localB->err;
13700 if (obj->MaxNumIterationInternal <= 0.0) {
13701 localB->exitFlag = IterationLimitExceeded;
13702 }
13703
13704 if ((localB->exitFlag == IterationLimitExceeded) || (localB->exitFlag ==
13705 TimeLimitExceeded)) {
13706 localB->exitFlagPrev = localB->exitFlag;
13707 exitg1 = true;
13708 } else {
13709 args = obj->ExtraArgs;
13710 obj_0 = args->Robot;
13711 localB->ix = newseed->size[0];
13712 newseed->size[0] = static_cast<int32_T>(obj_0->PositionNumber);
13713 cartes_emxEnsureCapacity_real_T(newseed, localB->ix, localB);
13714 localB->nx = static_cast<int32_T>(obj_0->PositionNumber);
13715 for (localB->ix = 0; localB->ix < localB->nx; localB->ix++) {
13716 newseed->data[localB->ix] = 0.0;
13717 }
13718
13719 localB->err = obj_0->NumBodies;
13720 localB->c_c = static_cast<int32_T>(localB->err) - 1;
13721 for (localB->i_fq = 0; localB->i_fq <= localB->c_c; localB->i_fq++) {
13722 localB->err = obj_0->PositionDoFMap[localB->i_fq];
13723 localB->iter = obj_0->PositionDoFMap[localB->i_fq + 8];
13724 if (localB->err <= localB->iter) {
13725 obj_1 = obj_0->Bodies[localB->i_fq]->JointInternal;
13726 if (static_cast<int32_T>(obj_1->PositionNumber) == 0) {
13727 localB->ix = qi->size[0] * qi->size[1];
13728 qi->size[0] = 1;
13729 qi->size[1] = 1;
13730 cartes_emxEnsureCapacity_real_T(qi, localB->ix, localB);
13731 qi->data[0] = (rtNaN);
13732 } else {
13733 localB->nx = obj_1->PositionLimitsInternal->size[0];
13734 localB->ix = ub->size[0];
13735 ub->size[0] = localB->nx;
13736 cartes_emxEnsureCapacity_real_T(ub, localB->ix, localB);
13737 for (localB->ix = 0; localB->ix < localB->nx; localB->ix++) {
13738 ub->data[localB->ix] = obj_1->PositionLimitsInternal->data
13739 [localB->ix + obj_1->PositionLimitsInternal->size[0]];
13740 }
13741
13742 localB->nx = obj_1->PositionLimitsInternal->size[0];
13743 localB->ix = lb->size[0];
13744 lb->size[0] = localB->nx;
13745 cartes_emxEnsureCapacity_real_T(lb, localB->ix, localB);
13746 for (localB->ix = 0; localB->ix < localB->nx; localB->ix++) {
13747 lb->data[localB->ix] = obj_1->PositionLimitsInternal->data
13748 [localB->ix];
13749 }
13750
13751 cartesian_trajectory_p_isfinite(lb, x_tmp);
13752 localB->y_pd = true;
13753 localB->ix = 0;
13754 exitg2 = false;
13755 while ((!exitg2) && (localB->ix + 1 <= x_tmp->size[0])) {
13756 if (!x_tmp->data[localB->ix]) {
13757 localB->y_pd = false;
13758 exitg2 = true;
13759 } else {
13760 localB->ix++;
13761 }
13762 }
13763
13764 guard1 = false;
13765 guard2 = false;
13766 guard3 = false;
13767 if (localB->y_pd) {
13768 cartesian_trajectory_p_isfinite(ub, x);
13769 localB->y_pd = true;
13770 localB->ix = 0;
13771 exitg2 = false;
13772 while ((!exitg2) && (localB->ix + 1 <= x->size[0])) {
13773 if (!x->data[localB->ix]) {
13774 localB->y_pd = false;
13775 exitg2 = true;
13776 } else {
13777 localB->ix++;
13778 }
13779 }
13780
13781 if (localB->y_pd) {
13782 cartesian_trajectory_pla_rand_m(obj_1->PositionNumber, rn,
13783 localB, localDW);
13784 localB->ix = qi->size[0] * qi->size[1];
13785 qi->size[0] = lb->size[0];
13786 qi->size[1] = 1;
13787 cartes_emxEnsureCapacity_real_T(qi, localB->ix, localB);
13788 localB->nx = lb->size[0] - 1;
13789 for (localB->ix = 0; localB->ix <= localB->nx; localB->ix++) {
13790 qi->data[localB->ix] = (ub->data[localB->ix] - lb->data
13791 [localB->ix]) * rn->data[localB->ix] + lb->data[localB->ix];
13792 }
13793 } else {
13794 guard3 = true;
13795 }
13796 } else {
13797 guard3 = true;
13798 }
13799
13800 if (guard3) {
13801 localB->y_pd = true;
13802 localB->ix = 0;
13803 exitg2 = false;
13804 while ((!exitg2) && (localB->ix + 1 <= x_tmp->size[0])) {
13805 if (!x_tmp->data[localB->ix]) {
13806 localB->y_pd = false;
13807 exitg2 = true;
13808 } else {
13809 localB->ix++;
13810 }
13811 }
13812
13813 if (localB->y_pd) {
13814 cartesian_trajectory_p_isfinite(ub, x);
13815 localB->ix = x_0->size[0];
13816 x_0->size[0] = x->size[0];
13817 car_emxEnsureCapacity_boolean_T(x_0, localB->ix);
13818 localB->nx = x->size[0];
13819 for (localB->ix = 0; localB->ix < localB->nx; localB->ix++) {
13820 x_0->data[localB->ix] = !x->data[localB->ix];
13821 }
13822
13823 if (cartesian_trajectory_planne_any(x_0)) {
13824 localB->ub[0] = lb->size[0];
13825 localB->ub[1] = 1.0;
13826 cartesian_trajectory_plan_randn(localB->ub, qi, localB,
13827 localDW);
13828 localB->nx = qi->size[0] - 1;
13829 localB->ix = e->size[0] * e->size[1];
13830 e->size[0] = qi->size[0];
13831 e->size[1] = 1;
13832 cartes_emxEnsureCapacity_real_T(e, localB->ix, localB);
13833 for (localB->ix = 0; localB->ix <= localB->nx; localB->ix++) {
13834 e->data[localB->ix] = fabs(qi->data[localB->ix]);
13835 }
13836
13837 localB->ix = qi->size[0] * qi->size[1];
13838 qi->size[0] = lb->size[0];
13839 qi->size[1] = 1;
13840 cartes_emxEnsureCapacity_real_T(qi, localB->ix, localB);
13841 localB->nx = lb->size[0] - 1;
13842 for (localB->ix = 0; localB->ix <= localB->nx; localB->ix++) {
13843 qi->data[localB->ix] = lb->data[localB->ix] + e->data
13844 [localB->ix];
13845 }
13846 } else {
13847 guard2 = true;
13848 }
13849 } else {
13850 guard2 = true;
13851 }
13852 }
13853
13854 if (guard2) {
13855 localB->ix = x_tmp_0->size[0];
13856 x_tmp_0->size[0] = x_tmp->size[0];
13857 car_emxEnsureCapacity_boolean_T(x_tmp_0, localB->ix);
13858 localB->nx = x_tmp->size[0];
13859 for (localB->ix = 0; localB->ix < localB->nx; localB->ix++) {
13860 x_tmp_0->data[localB->ix] = !x_tmp->data[localB->ix];
13861 }
13862
13863 if (cartesian_trajectory_planne_any(x_tmp_0)) {
13864 cartesian_trajectory_p_isfinite(ub, x);
13865 localB->y_pd = true;
13866 localB->ix = 0;
13867 exitg2 = false;
13868 while ((!exitg2) && (localB->ix + 1 <= x->size[0])) {
13869 if (!x->data[localB->ix]) {
13870 localB->y_pd = false;
13871 exitg2 = true;
13872 } else {
13873 localB->ix++;
13874 }
13875 }
13876
13877 if (localB->y_pd) {
13878 localB->ub[0] = ub->size[0];
13879 localB->ub[1] = 1.0;
13880 cartesian_trajectory_plan_randn(localB->ub, qi, localB,
13881 localDW);
13882 localB->nx = qi->size[0] - 1;
13883 localB->ix = e->size[0] * e->size[1];
13884 e->size[0] = qi->size[0];
13885 e->size[1] = 1;
13886 cartes_emxEnsureCapacity_real_T(e, localB->ix, localB);
13887 for (localB->ix = 0; localB->ix <= localB->nx; localB->ix++) {
13888 e->data[localB->ix] = fabs(qi->data[localB->ix]);
13889 }
13890
13891 localB->ix = qi->size[0] * qi->size[1];
13892 qi->size[0] = ub->size[0];
13893 qi->size[1] = 1;
13894 cartes_emxEnsureCapacity_real_T(qi, localB->ix, localB);
13895 localB->nx = ub->size[0] - 1;
13896 for (localB->ix = 0; localB->ix <= localB->nx; localB->ix++) {
13897 qi->data[localB->ix] = ub->data[localB->ix] - e->data
13898 [localB->ix];
13899 }
13900 } else {
13901 guard1 = true;
13902 }
13903 } else {
13904 guard1 = true;
13905 }
13906 }
13907
13908 if (guard1) {
13909 localB->ub[0] = ub->size[0];
13910 localB->ub[1] = 1.0;
13911 cartesian_trajectory_plan_randn(localB->ub, qi, localB, localDW);
13912 }
13913 }
13914
13915 if (localB->err > localB->iter) {
13916 localB->nx = 0;
13917 localB->ix = 0;
13918 } else {
13919 localB->nx = static_cast<int32_T>(localB->err) - 1;
13920 localB->ix = static_cast<int32_T>(localB->iter);
13921 }
13922
13923 localB->unnamed_idx_1 = localB->ix - localB->nx;
13924 for (localB->ix = 0; localB->ix < localB->unnamed_idx_1; localB->ix++)
13925 {
13926 newseed->data[localB->nx + localB->ix] = qi->data[localB->ix];
13927 }
13928 }
13929 }
13930
13931 for (localB->ix = 0; localB->ix < 6; localB->ix++) {
13932 obj->SeedInternal[localB->ix] = newseed->data[localB->ix];
13933 }
13934
13935 DampedBFGSwGradientProjection_s(obj, localB->c_xSol, &localB->exitFlag,
13936 &localB->err, &localB->iter, localB);
13937 if (localB->err < *solutionInfo_Error) {
13938 for (localB->i_fq = 0; localB->i_fq < 6; localB->i_fq++) {
13939 xSol[localB->i_fq] = localB->c_xSol[localB->i_fq];
13940 }
13941
13942 *solutionInfo_Error = localB->err;
13943 localB->exitFlagPrev = localB->exitFlag;
13944 }
13945
13946 (*solutionInfo_RRAttempts)++;
13947 *solutionInfo_Iterations += localB->iter;
13948 }
13949 }
13950
13951 cartesian_tra_emxFree_boolean_T(&x_0);
13952 cartesian_tra_emxFree_boolean_T(&x_tmp_0);
13953 cartesian_tra_emxFree_boolean_T(&x_tmp);
13954 cartesian_tra_emxFree_boolean_T(&x);
13955 cartesian_trajec_emxFree_real_T(&e);
13956 cartesian_trajec_emxFree_real_T(&rn);
13957 cartesian_trajec_emxFree_real_T(&lb);
13958 cartesian_trajec_emxFree_real_T(&ub);
13959 cartesian_trajec_emxFree_real_T(&qi);
13960 cartesian_trajec_emxFree_real_T(&newseed);
13961 *solutionInfo_ExitFlag = localB->exitFlagPrev;
13962 if (*solutionInfo_Error < localB->tol) {
13963 solutionInfo_Status_size[0] = 1;
13964 solutionInfo_Status_size[1] = 7;
13965 for (localB->ix = 0; localB->ix < 7; localB->ix++) {
13966 solutionInfo_Status_data[localB->ix] = tmp_0[localB->ix];
13967 }
13968 } else {
13969 solutionInfo_Status_size[0] = 1;
13970 solutionInfo_Status_size[1] = 14;
13971 for (localB->ix = 0; localB->ix < 14; localB->ix++) {
13972 solutionInfo_Status_data[localB->ix] = tmp[localB->ix];
13973 }
13974 }
13975}
13976
13977static void cart_inverseKinematics_stepImpl(b_inverseKinematics_cartesian_T *obj,
13978 const real_T tform[16], const real_T weights[6], const real_T initialGuess[6],
13979 real_T QSol[6], real_T *solutionInfo_Iterations, real_T
13980 *solutionInfo_PoseErrorNorm, real_T *solutionInfo_ExitFlag, char_T
13981 solutionInfo_Status_data[], int32_T solutionInfo_Status_size[2],
13982 B_MATLABSystem_cartesian_traj_T *localB, DW_MATLABSystem_cartesian_tra_T
13983 *localDW)
13984{
13985 emxArray_real_T_cartesian_tra_T *bodyIndices;
13986 emxArray_real_T_cartesian_tra_T *positionIndices;
13987 x_robotics_manip_internal_Rig_T *obj_0;
13988 emxArray_char_T_cartesian_tra_T *endEffectorName;
13989 w_robotics_manip_internal_Rig_T *body;
13990 emxArray_real_T_cartesian_tra_T *positionMap;
13991 emxArray_real_T_cartesian_tra_T *e;
13992 emxArray_int32_T_cartesian_tr_T *h;
13993 emxArray_real_T_cartesian_tra_T *y;
13994 emxArray_char_T_cartesian_tra_T *bname;
13995 emxArray_real_T_cartesian_tra_T *bodyIndices_0;
13996 boolean_T exitg1;
13997 c_inverseKinematics_setPoseGoal(obj, tform, weights, localB);
13998 for (localB->i_f = 0; localB->i_f < 6; localB->i_f++) {
13999 QSol[localB->i_f] = initialGuess[localB->i_f];
14000 }
14001
14002 cartesian_trajec_emxInit_char_T(&endEffectorName, 2, localB);
14003 RigidBodyTree_validateConfigu_j(obj->RigidBodyTreeInternal, QSol, localB);
14004 cartes_NLPSolverInterface_solve(obj->Solver, QSol, localB->qvSolRaw,
14005 &localB->d, &localB->bid, &localB->d1, &localB->d2, solutionInfo_Status_data,
14006 solutionInfo_Status_size, localB, localDW);
14007 *solutionInfo_ExitFlag = localB->d2;
14008 *solutionInfo_PoseErrorNorm = localB->d1;
14009 *solutionInfo_Iterations = localB->d;
14010 obj_0 = obj->RigidBodyTreeInternal;
14011 localB->partialTrueCount = endEffectorName->size[0] * endEffectorName->size[1];
14012 endEffectorName->size[0] = 1;
14013 endEffectorName->size[1] = obj->Solver->ExtraArgs->BodyName->size[1];
14014 cartes_emxEnsureCapacity_char_T(endEffectorName, localB->partialTrueCount,
14015 localB);
14016 localB->loop_ub = obj->Solver->ExtraArgs->BodyName->size[0] * obj->
14017 Solver->ExtraArgs->BodyName->size[1] - 1;
14018 for (localB->partialTrueCount = 0; localB->partialTrueCount <= localB->loop_ub;
14019 localB->partialTrueCount++) {
14020 endEffectorName->data[localB->partialTrueCount] = obj->Solver->
14021 ExtraArgs->BodyName->data[localB->partialTrueCount];
14022 }
14023
14024 cartesian_trajec_emxInit_real_T(&bodyIndices, 1, localB);
14025 localB->partialTrueCount = bodyIndices->size[0];
14026 bodyIndices->size[0] = static_cast<int32_T>(obj_0->NumBodies);
14027 cartes_emxEnsureCapacity_real_T(bodyIndices, localB->partialTrueCount, localB);
14028 localB->loop_ub = static_cast<int32_T>(obj_0->NumBodies);
14029 for (localB->partialTrueCount = 0; localB->partialTrueCount < localB->loop_ub;
14030 localB->partialTrueCount++) {
14031 bodyIndices->data[localB->partialTrueCount] = 0.0;
14032 }
14033
14034 cartesian_trajec_emxInit_char_T(&bname, 2, localB);
14035 localB->bid = -1.0;
14036 localB->partialTrueCount = bname->size[0] * bname->size[1];
14037 bname->size[0] = 1;
14038 bname->size[1] = obj_0->Base.NameInternal->size[1];
14039 cartes_emxEnsureCapacity_char_T(bname, localB->partialTrueCount, localB);
14040 localB->loop_ub = obj_0->Base.NameInternal->size[0] * obj_0->
14041 Base.NameInternal->size[1] - 1;
14042 for (localB->partialTrueCount = 0; localB->partialTrueCount <= localB->loop_ub;
14043 localB->partialTrueCount++) {
14044 bname->data[localB->partialTrueCount] = obj_0->Base.NameInternal->
14045 data[localB->partialTrueCount];
14046 }
14047
14048 if (cartesian_trajectory_pla_strcmp(bname, endEffectorName)) {
14049 localB->bid = 0.0;
14050 } else {
14051 localB->numPositions = obj_0->NumBodies;
14052 localB->i_f = 0;
14053 exitg1 = false;
14054 while ((!exitg1) && (localB->i_f <= static_cast<int32_T>
14055 (localB->numPositions) - 1)) {
14056 body = obj_0->Bodies[localB->i_f];
14057 localB->partialTrueCount = bname->size[0] * bname->size[1];
14058 bname->size[0] = 1;
14059 bname->size[1] = body->NameInternal->size[1];
14060 cartes_emxEnsureCapacity_char_T(bname, localB->partialTrueCount, localB);
14061 localB->loop_ub = body->NameInternal->size[0] * body->NameInternal->size[1]
14062 - 1;
14063 for (localB->partialTrueCount = 0; localB->partialTrueCount <=
14064 localB->loop_ub; localB->partialTrueCount++) {
14065 bname->data[localB->partialTrueCount] = body->NameInternal->data
14066 [localB->partialTrueCount];
14067 }
14068
14069 if (cartesian_trajectory_pla_strcmp(bname, endEffectorName)) {
14070 localB->bid = static_cast<real_T>(localB->i_f) + 1.0;
14071 exitg1 = true;
14072 } else {
14073 localB->i_f++;
14074 }
14075 }
14076 }
14077
14078 cartesian_trajec_emxFree_char_T(&bname);
14079 cartesian_trajec_emxFree_char_T(&endEffectorName);
14080 if (localB->bid == 0.0) {
14081 localB->partialTrueCount = bodyIndices->size[0];
14082 bodyIndices->size[0] = 1;
14083 cartes_emxEnsureCapacity_real_T(bodyIndices, localB->partialTrueCount,
14084 localB);
14085 bodyIndices->data[0] = 0.0;
14086 } else {
14087 body = obj_0->Bodies[static_cast<int32_T>(localB->bid) - 1];
14088 localB->bid = 1.0;
14089 while (body->ParentIndex != 0.0) {
14090 bodyIndices->data[static_cast<int32_T>(localB->bid) - 1] = body->Index;
14091 body = obj_0->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
14092 localB->bid++;
14093 }
14094
14095 if (1.0 > localB->bid - 1.0) {
14096 localB->c_k = -1;
14097 } else {
14098 localB->c_k = static_cast<int32_T>(localB->bid - 1.0) - 1;
14099 }
14100
14101 cartesian_trajec_emxInit_real_T(&bodyIndices_0, 1, localB);
14102 localB->partialTrueCount = bodyIndices_0->size[0];
14103 bodyIndices_0->size[0] = localB->c_k + 3;
14104 cartes_emxEnsureCapacity_real_T(bodyIndices_0, localB->partialTrueCount,
14105 localB);
14106 for (localB->partialTrueCount = 0; localB->partialTrueCount <= localB->c_k;
14107 localB->partialTrueCount++) {
14108 bodyIndices_0->data[localB->partialTrueCount] = bodyIndices->data
14109 [localB->partialTrueCount];
14110 }
14111
14112 bodyIndices_0->data[localB->c_k + 1] = body->Index;
14113 bodyIndices_0->data[localB->c_k + 2] = 0.0;
14114 localB->partialTrueCount = bodyIndices->size[0];
14115 bodyIndices->size[0] = bodyIndices_0->size[0];
14116 cartes_emxEnsureCapacity_real_T(bodyIndices, localB->partialTrueCount,
14117 localB);
14118 localB->loop_ub = bodyIndices_0->size[0];
14119 for (localB->partialTrueCount = 0; localB->partialTrueCount <
14120 localB->loop_ub; localB->partialTrueCount++) {
14121 bodyIndices->data[localB->partialTrueCount] = bodyIndices_0->data
14122 [localB->partialTrueCount];
14123 }
14124
14125 cartesian_trajec_emxFree_real_T(&bodyIndices_0);
14126 }
14127
14128 obj_0 = obj->RigidBodyTreeInternal;
14129 localB->c_k = bodyIndices->size[0] - 1;
14130 localB->loop_ub = 0;
14131 for (localB->i_f = 0; localB->i_f <= localB->c_k; localB->i_f++) {
14132 if (bodyIndices->data[localB->i_f] != 0.0) {
14133 localB->loop_ub++;
14134 }
14135 }
14136
14137 cartesian_traje_emxInit_int32_T(&h, 1);
14138 localB->partialTrueCount = h->size[0];
14139 h->size[0] = localB->loop_ub;
14140 carte_emxEnsureCapacity_int32_T(h, localB->partialTrueCount);
14141 localB->partialTrueCount = 0;
14142 for (localB->i_f = 0; localB->i_f <= localB->c_k; localB->i_f++) {
14143 if (bodyIndices->data[localB->i_f] != 0.0) {
14144 h->data[localB->partialTrueCount] = localB->i_f + 1;
14145 localB->partialTrueCount++;
14146 }
14147 }
14148
14149 cartesian_trajec_emxInit_real_T(&positionMap, 2, localB);
14150 localB->partialTrueCount = positionMap->size[0] * positionMap->size[1];
14151 positionMap->size[0] = h->size[0];
14152 positionMap->size[1] = 2;
14153 cartes_emxEnsureCapacity_real_T(positionMap, localB->partialTrueCount, localB);
14154 localB->loop_ub = h->size[0];
14155 for (localB->partialTrueCount = 0; localB->partialTrueCount < localB->loop_ub;
14156 localB->partialTrueCount++) {
14157 positionMap->data[localB->partialTrueCount] = obj_0->PositionDoFMap[
14158 static_cast<int32_T>(bodyIndices->data[h->data[localB->partialTrueCount] -
14159 1]) - 1];
14160 }
14161
14162 localB->loop_ub = h->size[0];
14163 for (localB->partialTrueCount = 0; localB->partialTrueCount < localB->loop_ub;
14164 localB->partialTrueCount++) {
14165 positionMap->data[localB->partialTrueCount + positionMap->size[0]] =
14166 obj_0->PositionDoFMap[static_cast<int32_T>(bodyIndices->data[h->
14167 data[localB->partialTrueCount] - 1]) + 7];
14168 }
14169
14170 cartesian_traje_emxFree_int32_T(&h);
14171 cartesian_trajec_emxFree_real_T(&bodyIndices);
14172 cartesian_trajec_emxInit_real_T(&positionIndices, 2, localB);
14173 localB->partialTrueCount = positionIndices->size[0] * positionIndices->size[1];
14174 positionIndices->size[0] = 1;
14175 positionIndices->size[1] = static_cast<int32_T>(obj_0->PositionNumber);
14176 cartes_emxEnsureCapacity_real_T(positionIndices, localB->partialTrueCount,
14177 localB);
14178 localB->loop_ub = static_cast<int32_T>(obj_0->PositionNumber) - 1;
14179 for (localB->partialTrueCount = 0; localB->partialTrueCount <= localB->loop_ub;
14180 localB->partialTrueCount++) {
14181 positionIndices->data[localB->partialTrueCount] = 0.0;
14182 }
14183
14184 localB->bid = 0.0;
14185 localB->c_k = positionMap->size[0] - 1;
14186 cartesian_trajec_emxInit_real_T(&e, 2, localB);
14187 cartesian_trajec_emxInit_real_T(&y, 2, localB);
14188 for (localB->i_f = 0; localB->i_f <= localB->c_k; localB->i_f++) {
14189 localB->numPositions = (positionMap->data[localB->i_f + positionMap->size[0]]
14190 - positionMap->data[localB->i_f]) + 1.0;
14191 if (localB->numPositions > 0.0) {
14192 if (localB->numPositions < 1.0) {
14193 y->size[0] = 1;
14194 y->size[1] = 0;
14195 } else if (rtIsInf(localB->numPositions) && (1.0 == localB->numPositions))
14196 {
14197 localB->partialTrueCount = y->size[0] * y->size[1];
14198 y->size[0] = 1;
14199 y->size[1] = 1;
14200 cartes_emxEnsureCapacity_real_T(y, localB->partialTrueCount, localB);
14201 y->data[0] = (rtNaN);
14202 } else {
14203 localB->partialTrueCount = y->size[0] * y->size[1];
14204 y->size[0] = 1;
14205 localB->loop_ub = static_cast<int32_T>(floor(localB->numPositions - 1.0));
14206 y->size[1] = localB->loop_ub + 1;
14207 cartes_emxEnsureCapacity_real_T(y, localB->partialTrueCount, localB);
14208 for (localB->partialTrueCount = 0; localB->partialTrueCount <=
14209 localB->loop_ub; localB->partialTrueCount++) {
14210 y->data[localB->partialTrueCount] = static_cast<real_T>
14211 (localB->partialTrueCount) + 1.0;
14212 }
14213 }
14214
14215 if (rtIsNaN(positionMap->data[localB->i_f]) || rtIsNaN(positionMap->
14216 data[localB->i_f + positionMap->size[0]])) {
14217 localB->partialTrueCount = e->size[0] * e->size[1];
14218 e->size[0] = 1;
14219 e->size[1] = 1;
14220 cartes_emxEnsureCapacity_real_T(e, localB->partialTrueCount, localB);
14221 e->data[0] = (rtNaN);
14222 } else if (positionMap->data[localB->i_f + positionMap->size[0]] <
14223 positionMap->data[localB->i_f]) {
14224 e->size[0] = 1;
14225 e->size[1] = 0;
14226 } else if ((rtIsInf(positionMap->data[localB->i_f]) || rtIsInf
14227 (positionMap->data[localB->i_f + positionMap->size[0]])) &&
14228 (positionMap->data[localB->i_f + positionMap->size[0]] ==
14229 positionMap->data[localB->i_f])) {
14230 localB->partialTrueCount = e->size[0] * e->size[1];
14231 e->size[0] = 1;
14232 e->size[1] = 1;
14233 cartes_emxEnsureCapacity_real_T(e, localB->partialTrueCount, localB);
14234 e->data[0] = (rtNaN);
14235 } else if (floor(positionMap->data[localB->i_f]) == positionMap->
14236 data[localB->i_f]) {
14237 localB->partialTrueCount = e->size[0] * e->size[1];
14238 e->size[0] = 1;
14239 e->size[1] = static_cast<int32_T>(floor(positionMap->data[localB->i_f +
14240 positionMap->size[0]] - positionMap->data[localB->i_f])) + 1;
14241 cartes_emxEnsureCapacity_real_T(e, localB->partialTrueCount, localB);
14242 localB->loop_ub = static_cast<int32_T>(floor(positionMap->data
14243 [localB->i_f + positionMap->size[0]] - positionMap->data[localB->i_f]));
14244 for (localB->partialTrueCount = 0; localB->partialTrueCount <=
14245 localB->loop_ub; localB->partialTrueCount++) {
14246 e->data[localB->partialTrueCount] = positionMap->data[localB->i_f] +
14247 static_cast<real_T>(localB->partialTrueCount);
14248 }
14249 } else {
14250 localB->ndbl = floor((positionMap->data[localB->i_f + positionMap->size
14251 [0]] - positionMap->data[localB->i_f]) + 0.5);
14252 localB->apnd = positionMap->data[localB->i_f] + localB->ndbl;
14253 localB->cdiff = localB->apnd - positionMap->data[localB->i_f +
14254 positionMap->size[0]];
14255 localB->u0_m = fabs(positionMap->data[localB->i_f]);
14256 localB->u1_m = fabs(positionMap->data[localB->i_f + positionMap->size[0]]);
14257 if ((localB->u0_m > localB->u1_m) || rtIsNaN(localB->u1_m)) {
14258 localB->u1_m = localB->u0_m;
14259 }
14260
14261 if (fabs(localB->cdiff) < 4.4408920985006262E-16 * localB->u1_m) {
14262 localB->ndbl++;
14263 localB->apnd = positionMap->data[localB->i_f + positionMap->size[0]];
14264 } else if (localB->cdiff > 0.0) {
14265 localB->apnd = (localB->ndbl - 1.0) + positionMap->data[localB->i_f];
14266 } else {
14267 localB->ndbl++;
14268 }
14269
14270 if (localB->ndbl >= 0.0) {
14271 localB->partialTrueCount = static_cast<int32_T>(localB->ndbl);
14272 } else {
14273 localB->partialTrueCount = 0;
14274 }
14275
14276 localB->loop_ub = localB->partialTrueCount - 1;
14277 localB->partialTrueCount = e->size[0] * e->size[1];
14278 e->size[0] = 1;
14279 e->size[1] = localB->loop_ub + 1;
14280 cartes_emxEnsureCapacity_real_T(e, localB->partialTrueCount, localB);
14281 if (localB->loop_ub + 1 > 0) {
14282 e->data[0] = positionMap->data[localB->i_f];
14283 if (localB->loop_ub + 1 > 1) {
14284 e->data[localB->loop_ub] = localB->apnd;
14285 localB->nm1d2 = ((localB->loop_ub < 0) + localB->loop_ub) >> 1;
14286 localB->c_j = localB->nm1d2 - 2;
14287 for (localB->partialTrueCount = 0; localB->partialTrueCount <=
14288 localB->c_j; localB->partialTrueCount++) {
14289 localB->k = localB->partialTrueCount + 1;
14290 e->data[localB->k] = positionMap->data[localB->i_f] + static_cast<
14291 real_T>(localB->k);
14292 e->data[localB->loop_ub - localB->k] = localB->apnd - static_cast<
14293 real_T>(localB->k);
14294 }
14295
14296 if (localB->nm1d2 << 1 == localB->loop_ub) {
14297 e->data[localB->nm1d2] = (positionMap->data[localB->i_f] +
14298 localB->apnd) / 2.0;
14299 } else {
14300 e->data[localB->nm1d2] = positionMap->data[localB->i_f] +
14301 static_cast<real_T>(localB->nm1d2);
14302 e->data[localB->nm1d2 + 1] = localB->apnd - static_cast<real_T>
14303 (localB->nm1d2);
14304 }
14305 }
14306 }
14307 }
14308
14309 localB->partialTrueCount = e->size[0] * e->size[1];
14310 localB->loop_ub = localB->partialTrueCount - 1;
14311 for (localB->partialTrueCount = 0; localB->partialTrueCount <=
14312 localB->loop_ub; localB->partialTrueCount++) {
14313 positionIndices->data[static_cast<int32_T>(localB->bid + y->data
14314 [localB->partialTrueCount]) - 1] = e->data[localB->partialTrueCount];
14315 }
14316
14317 localB->bid += localB->numPositions;
14318 }
14319 }
14320
14321 cartesian_trajec_emxFree_real_T(&y);
14322 cartesian_trajec_emxFree_real_T(&e);
14323 cartesian_trajec_emxFree_real_T(&positionMap);
14324 if (1.0 > localB->bid) {
14325 positionIndices->size[1] = 0;
14326 } else {
14327 localB->partialTrueCount = positionIndices->size[0] * positionIndices->size
14328 [1];
14329 positionIndices->size[1] = static_cast<int32_T>(localB->bid);
14330 cartes_emxEnsureCapacity_real_T(positionIndices, localB->partialTrueCount,
14331 localB);
14332 }
14333
14334 localB->loop_ub = positionIndices->size[0] * positionIndices->size[1];
14335 for (localB->partialTrueCount = 0; localB->partialTrueCount < localB->loop_ub;
14336 localB->partialTrueCount++) {
14337 QSol[static_cast<int32_T>(positionIndices->data[localB->partialTrueCount]) -
14338 1] = localB->qvSolRaw[static_cast<int32_T>(positionIndices->data
14339 [localB->partialTrueCount]) - 1];
14340 }
14341
14342 cartesian_trajec_emxFree_real_T(&positionIndices);
14343}
14344
14345real_T rt_roundd_snf(real_T u)
14346{
14347 real_T y;
14348 if (fabs(u) < 4.503599627370496E+15) {
14349 if (u >= 0.5) {
14350 y = floor(u + 0.5);
14351 } else if (u > -0.5) {
14352 y = u * 0.0;
14353 } else {
14354 y = ceil(u - 0.5);
14355 }
14356 } else {
14357 y = u;
14358 }
14359
14360 return y;
14361}
14362
14363static void matlabCodegenHandle_matlabCod_o(robotics_slmanip_internal_blo_T *obj)
14364{
14365 if (!obj->matlabCodegenIsDeleted) {
14366 obj->matlabCodegenIsDeleted = true;
14367 }
14368}
14369
14370static void cartesian_tr_SystemCore_release(b_inverseKinematics_cartesian_T *obj)
14371{
14372 if (obj->isInitialized == 1) {
14373 obj->isInitialized = 2;
14374 }
14375}
14376
14377static void cartesian_tra_SystemCore_delete(b_inverseKinematics_cartesian_T *obj)
14378{
14379 cartesian_tr_SystemCore_release(obj);
14380}
14381
14382static void matlabCodegenHandle_matlabCodeg(b_inverseKinematics_cartesian_T *obj)
14383{
14384 if (!obj->matlabCodegenIsDeleted) {
14385 obj->matlabCodegenIsDeleted = true;
14386 cartesian_tra_SystemCore_delete(obj);
14387 }
14388}
14389
14390static void emxFreeStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_tr_T
14391 *pStruct)
14392{
14393 cartesian_trajec_emxFree_char_T(&pStruct->Type);
14394 cartesian_trajec_emxFree_real_T(&pStruct->MotionSubspace);
14395 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
14396 cartesian_trajec_emxFree_real_T(&pStruct->PositionLimitsInternal);
14397 cartesian_trajec_emxFree_real_T(&pStruct->HomePositionInternal);
14398}
14399
14400static void emxFreeStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
14401 *pStruct)
14402{
14403 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
14404 emxFreeStruct_c_rigidBodyJoint(&pStruct->JointInternal);
14405}
14406
14407static void emxFreeStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
14408 *pStruct)
14409{
14410 emxFreeStruct_v_robotics_manip_(&pStruct->Base);
14411}
14412
14413static void emxFreeStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
14414 *pStruct)
14415{
14416 cartesian_trajec_emxFree_real_T(&pStruct->Limits);
14417}
14418
14419static void emxFreeStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T
14420 *pStruct)
14421{
14422 emxFreeStruct_y_robotics_manip_(&pStruct->TreeInternal);
14423 emxFreeStruct_b_inverseKinemati(&pStruct->IKInternal);
14424}
14425
14426static void emxFreeStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
14427 *pStruct)
14428{
14429 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
14430}
14431
14432static void emxFreeStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
14433 *pStruct)
14434{
14435 emxFreeStruct_w_robotics_manip_(&pStruct->Base);
14436}
14437
14438static void emxFreeStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
14439 *pStruct)
14440{
14441 cartesian_trajec_emxFree_char_T(&pStruct->BodyName);
14442 cartesian_trajec_emxFree_real_T(&pStruct->ErrTemp);
14443 cartesian_trajec_emxFree_real_T(&pStruct->GradTemp);
14444}
14445
14446static void emxFreeStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
14447 *pStruct)
14448{
14449 cartesian_trajec_emxFree_real_T(&pStruct->ConstraintMatrix);
14450 cartesian_trajec_emxFree_real_T(&pStruct->ConstraintBound);
14451}
14452
14453//
14454// System initialize for atomic system:
14455// synthesized block
14456// synthesized block
14457//
14458void cartesian_tra_MATLABSystem_Init(B_MATLABSystem_cartesian_traj_T *localB,
14459 DW_MATLABSystem_cartesian_tra_T *localDW)
14460{
14461 robotics_slmanip_internal_blo_T *obj;
14462 b_inverseKinematics_cartesian_T *obj_0;
14463 h_robotics_core_internal_Damp_T *obj_1;
14464 emxInitStruct_robotics_slmanip_(&localDW->obj, localB);
14465 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_1, localB);
14466 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_50, localB);
14467 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_49, localB);
14468 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_48, localB);
14469 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_47, localB);
14470 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_46, localB);
14471 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_45, localB);
14472 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_44, localB);
14473 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_43, localB);
14474 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_42, localB);
14475 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_41, localB);
14476 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_40, localB);
14477 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_39, localB);
14478 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_38, localB);
14479 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_37, localB);
14480 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_36, localB);
14481 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_35, localB);
14482 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_34, localB);
14483 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_33, localB);
14484 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_32, localB);
14485 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_31, localB);
14486 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_30, localB);
14487 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_29, localB);
14488 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_28, localB);
14489 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_27, localB);
14490 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_26, localB);
14491 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_25, localB);
14492 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_24, localB);
14493 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_23, localB);
14494 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_22, localB);
14495 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_21, localB);
14496 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_20, localB);
14497 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_19, localB);
14498 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_18, localB);
14499 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_17, localB);
14500 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_16, localB);
14501 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_15, localB);
14502 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_14, localB);
14503 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_13, localB);
14504 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_12, localB);
14505 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_11, localB);
14506 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_10, localB);
14507 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_9, localB);
14508 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_8, localB);
14509 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_7, localB);
14510 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_6, localB);
14511 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_5, localB);
14512 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_4, localB);
14513 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_3, localB);
14514 emxInitStruct_c_rigidBodyJoint(&localDW->gobj_2, localB);
14515 emxInitStruct_w_robotics_manip_(&localDW->gobj_51, localB);
14516 emxInitStruct_w_robotics_manip_(&localDW->gobj_82, localB);
14517 emxInitStruct_w_robotics_manip_(&localDW->gobj_81, localB);
14518 emxInitStruct_w_robotics_manip_(&localDW->gobj_80, localB);
14519 emxInitStruct_w_robotics_manip_(&localDW->gobj_79, localB);
14520 emxInitStruct_w_robotics_manip_(&localDW->gobj_78, localB);
14521 emxInitStruct_w_robotics_manip_(&localDW->gobj_77, localB);
14522 emxInitStruct_w_robotics_manip_(&localDW->gobj_76, localB);
14523 emxInitStruct_w_robotics_manip_(&localDW->gobj_75, localB);
14524 emxInitStruct_w_robotics_manip_(&localDW->gobj_74, localB);
14525 emxInitStruct_w_robotics_manip_(&localDW->gobj_73, localB);
14526 emxInitStruct_w_robotics_manip_(&localDW->gobj_72, localB);
14527 emxInitStruct_w_robotics_manip_(&localDW->gobj_71, localB);
14528 emxInitStruct_w_robotics_manip_(&localDW->gobj_70, localB);
14529 emxInitStruct_w_robotics_manip_(&localDW->gobj_69, localB);
14530 emxInitStruct_w_robotics_manip_(&localDW->gobj_68, localB);
14531 emxInitStruct_w_robotics_manip_(&localDW->gobj_67, localB);
14532 emxInitStruct_w_robotics_manip_(&localDW->gobj_66, localB);
14533 emxInitStruct_w_robotics_manip_(&localDW->gobj_65, localB);
14534 emxInitStruct_w_robotics_manip_(&localDW->gobj_64, localB);
14535 emxInitStruct_w_robotics_manip_(&localDW->gobj_63, localB);
14536 emxInitStruct_w_robotics_manip_(&localDW->gobj_62, localB);
14537 emxInitStruct_w_robotics_manip_(&localDW->gobj_61, localB);
14538 emxInitStruct_w_robotics_manip_(&localDW->gobj_60, localB);
14539 emxInitStruct_w_robotics_manip_(&localDW->gobj_59, localB);
14540 emxInitStruct_w_robotics_manip_(&localDW->gobj_58, localB);
14541 emxInitStruct_w_robotics_manip_(&localDW->gobj_57, localB);
14542 emxInitStruct_w_robotics_manip_(&localDW->gobj_56, localB);
14543 emxInitStruct_w_robotics_manip_(&localDW->gobj_55, localB);
14544 emxInitStruct_w_robotics_manip_(&localDW->gobj_54, localB);
14545 emxInitStruct_w_robotics_manip_(&localDW->gobj_53, localB);
14546 emxInitStruct_w_robotics_manip_(&localDW->gobj_52, localB);
14547 emxInitStruct_x_robotics_manip_(&localDW->gobj_83, localB);
14548 emxInitStruct_x_robotics_manip_(&localDW->gobj_84, localB);
14549 emxInitStruct_f_robotics_manip_(&localDW->gobj_85, localB);
14550 emxInitStruct_h_robotics_core_i(&localDW->gobj_86, localB);
14551 emxInitStruct_h_robotics_core_i(&localDW->gobj_87, localB);
14552 emxInitStruct_v_robotics_manip_(&localDW->gobj_88, localB);
14553 emxInitStruct_v_robotics_manip_(&localDW->gobj_103, localB);
14554 emxInitStruct_v_robotics_manip_(&localDW->gobj_102, localB);
14555 emxInitStruct_v_robotics_manip_(&localDW->gobj_101, localB);
14556 emxInitStruct_v_robotics_manip_(&localDW->gobj_100, localB);
14557 emxInitStruct_v_robotics_manip_(&localDW->gobj_99, localB);
14558 emxInitStruct_v_robotics_manip_(&localDW->gobj_98, localB);
14559 emxInitStruct_v_robotics_manip_(&localDW->gobj_97, localB);
14560 emxInitStruct_v_robotics_manip_(&localDW->gobj_96, localB);
14561 emxInitStruct_v_robotics_manip_(&localDW->gobj_95, localB);
14562 emxInitStruct_v_robotics_manip_(&localDW->gobj_94, localB);
14563 emxInitStruct_v_robotics_manip_(&localDW->gobj_93, localB);
14564 emxInitStruct_v_robotics_manip_(&localDW->gobj_92, localB);
14565 emxInitStruct_v_robotics_manip_(&localDW->gobj_91, localB);
14566 emxInitStruct_v_robotics_manip_(&localDW->gobj_90, localB);
14567 emxInitStruct_v_robotics_manip_(&localDW->gobj_89, localB);
14568
14569 // Start for MATLABSystem: '<S2>/MATLAB System'
14570 localDW->obj.IKInternal.matlabCodegenIsDeleted = true;
14571 localDW->obj.matlabCodegenIsDeleted = true;
14572 localDW->method_m = 7U;
14573 localDW->method_not_empty = true;
14574 localDW->state = 1144108930U;
14575 localDW->state_not_empty = true;
14576 localDW->state_a[0] = 362436069U;
14577 localDW->state_a[1] = 521288629U;
14578 localDW->state_not_empty_k = true;
14579 cartesian_tr_eml_rand_mt19937ar(localDW->state_e);
14580 localDW->method_not_empty_k = true;
14581 localDW->state_not_empty_j = true;
14582 cartesian_tra_eml_rand_shr3cong(localDW->state_ar);
14583 localDW->state_not_empty_i = true;
14584 localDW->obj.isInitialized = 0;
14585 localDW->obj.matlabCodegenIsDeleted = false;
14586 localDW->objisempty = true;
14587 obj = &localDW->obj;
14588 localDW->obj.isInitialized = 1;
14589 car_RigidBodyTree_RigidBodyTree(&localDW->obj.TreeInternal, &localDW->gobj_90,
14590 &localDW->gobj_91, &localDW->gobj_92, &localDW->gobj_93, &localDW->gobj_94,
14591 &localDW->gobj_95, &localDW->gobj_96, &localDW->gobj_89, localB);
14592 obj_0 = &localDW->obj.IKInternal;
14593 localDW->obj.IKInternal.isInitialized = 0;
14594 inverseKinematics_set_RigidBody(&localDW->obj.IKInternal, &obj->TreeInternal,
14595 &localDW->gobj_69, &localDW->gobj_70, &localDW->gobj_71, &localDW->gobj_72,
14596 &localDW->gobj_73, &localDW->gobj_74, &localDW->gobj_75, &localDW->gobj_76,
14597 &localDW->gobj_77, &localDW->gobj_78, &localDW->gobj_79, &localDW->gobj_80,
14598 &localDW->gobj_81, &localDW->gobj_82, &localDW->gobj_51, &localDW->gobj_28,
14599 &localDW->gobj_29, &localDW->gobj_30, &localDW->gobj_31, &localDW->gobj_32,
14600 &localDW->gobj_33, &localDW->gobj_34, &localDW->gobj_35, &localDW->gobj_36,
14601 &localDW->gobj_37, &localDW->gobj_38, &localDW->gobj_39, &localDW->gobj_40,
14602 &localDW->gobj_41, &localDW->gobj_42, &localDW->gobj_43, &localDW->gobj_44,
14603 &localDW->gobj_45, &localDW->gobj_46, &localDW->gobj_47, &localDW->gobj_48,
14604 &localDW->gobj_49, &localDW->gobj_50, &localDW->gobj_1, &localDW->gobj_27,
14605 &localDW->gobj_68, &localDW->gobj_83, localB, localDW);
14606 obj_0->Solver = DampedBFGSwGradientProjection_D(&localDW->gobj_87);
14607 obj_1 = obj_0->Solver;
14608 obj_1->MaxNumIteration = 1500.0;
14609 obj_1->MaxTime = 10.0;
14610 obj_1->GradientTolerance = 1.0E-7;
14611 obj_1->SolutionTolerance = 1.0E-6;
14612 obj_1->ConstraintsOn = true;
14613 obj_1->RandomRestart = false;
14614 obj_1->StepTolerance = 1.0E-14;
14615 obj_0->matlabCodegenIsDeleted = false;
14616}
14617
14618//
14619// Output and update for atomic system:
14620// synthesized block
14621// synthesized block
14622//
14623void cartesian_trajecto_MATLABSystem(const real_T rtu_0[16], const real_T rtu_1
14624 [6], const real_T rtu_2[6], B_MATLABSystem_cartesian_traj_T *localB,
14625 DW_MATLABSystem_cartesian_tra_T *localDW)
14626{
14627 robotics_slmanip_internal_blo_T *obj;
14628 static const char_T tmp[7] = { 's', 'u', 'c', 'c', 'e', 's', 's' };
14629
14630 int32_T exitg1;
14631
14632 // MATLABSystem: '<S2>/MATLAB System'
14633 for (localB->i = 0; localB->i < 16; localB->i++) {
14634 localB->u0[localB->i] = rtu_0[localB->i];
14635 }
14636
14637 for (localB->i = 0; localB->i < 6; localB->i++) {
14638 localB->u1[localB->i] = rtu_1[localB->i];
14639 }
14640
14641 for (localB->i = 0; localB->i < 6; localB->i++) {
14642 localB->u2[localB->i] = rtu_2[localB->i];
14643 }
14644
14645 obj = &localDW->obj;
14646 if (localDW->obj.IKInternal.isInitialized != 1) {
14647 localDW->obj.IKInternal.isSetupComplete = false;
14648 localDW->obj.IKInternal.isInitialized = 1;
14649 car_inverseKinematics_setupImpl(&localDW->obj.IKInternal, &localDW->gobj_85,
14650 localB);
14651 obj->IKInternal.isSetupComplete = true;
14652 }
14653
14654 cart_inverseKinematics_stepImpl(&obj->IKInternal, localB->u0, localB->u1,
14655 localB->u2, localB->MATLABSystem_o1, &localB->MATLABSystem_o2.Iterations,
14656 &localB->MATLABSystem_o2.PoseErrorNorm, &localB->b_varargout_2_ExitFlag,
14657 localB->b_varargout_2_Status_data, localB->b_varargout_2_Status_size, localB,
14658 localDW);
14659 for (localB->i = 0; localB->i < 7; localB->i++) {
14660 localB->b_j[localB->i] = tmp[localB->i];
14661 }
14662
14663 localB->b_bool = false;
14664 if (localB->b_varargout_2_Status_size[1] == 7) {
14665 localB->i = 1;
14666 do {
14667 exitg1 = 0;
14668 if (localB->i - 1 < 7) {
14669 localB->kstr = localB->i - 1;
14670 if (localB->b_varargout_2_Status_data[localB->kstr] != localB->
14671 b_j[localB->kstr]) {
14672 exitg1 = 1;
14673 } else {
14674 localB->i++;
14675 }
14676 } else {
14677 localB->b_bool = true;
14678 exitg1 = 1;
14679 }
14680 } while (exitg1 == 0);
14681 }
14682
14683 localB->b_varargout_2_ExitFlag = rt_roundd_snf(localB->b_varargout_2_ExitFlag);
14684 if (localB->b_varargout_2_ExitFlag < 65536.0) {
14685 if (localB->b_varargout_2_ExitFlag >= 0.0) {
14686 localB->MATLABSystem_o2.ExitFlag = static_cast<uint16_T>
14687 (localB->b_varargout_2_ExitFlag);
14688 } else {
14689 localB->MATLABSystem_o2.ExitFlag = 0U;
14690 }
14691 } else {
14692 localB->MATLABSystem_o2.ExitFlag = MAX_uint16_T;
14693 }
14694
14695 if (localB->b_bool) {
14696 localB->MATLABSystem_o2.Status = 1U;
14697 } else {
14698 localB->MATLABSystem_o2.Status = 2U;
14699 }
14700
14701 // End of MATLABSystem: '<S2>/MATLAB System'
14702}
14703
14704//
14705// Termination for atomic system:
14706// synthesized block
14707// synthesized block
14708//
14709void cartesian_tra_MATLABSystem_Term(DW_MATLABSystem_cartesian_tra_T *localDW)
14710{
14711 // Terminate for MATLABSystem: '<S2>/MATLAB System'
14712 matlabCodegenHandle_matlabCod_o(&localDW->obj);
14713 matlabCodegenHandle_matlabCodeg(&localDW->obj.IKInternal);
14714 emxFreeStruct_robotics_slmanip_(&localDW->obj);
14715 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_1);
14716 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_50);
14717 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_49);
14718 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_48);
14719 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_47);
14720 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_46);
14721 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_45);
14722 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_44);
14723 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_43);
14724 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_42);
14725 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_41);
14726 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_40);
14727 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_39);
14728 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_38);
14729 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_37);
14730 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_36);
14731 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_35);
14732 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_34);
14733 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_33);
14734 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_32);
14735 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_31);
14736 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_30);
14737 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_29);
14738 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_28);
14739 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_27);
14740 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_26);
14741 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_25);
14742 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_24);
14743 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_23);
14744 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_22);
14745 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_21);
14746 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_20);
14747 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_19);
14748 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_18);
14749 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_17);
14750 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_16);
14751 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_15);
14752 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_14);
14753 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_13);
14754 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_12);
14755 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_11);
14756 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_10);
14757 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_9);
14758 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_8);
14759 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_7);
14760 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_6);
14761 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_5);
14762 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_4);
14763 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_3);
14764 emxFreeStruct_c_rigidBodyJoint(&localDW->gobj_2);
14765 emxFreeStruct_w_robotics_manip_(&localDW->gobj_51);
14766 emxFreeStruct_w_robotics_manip_(&localDW->gobj_82);
14767 emxFreeStruct_w_robotics_manip_(&localDW->gobj_81);
14768 emxFreeStruct_w_robotics_manip_(&localDW->gobj_80);
14769 emxFreeStruct_w_robotics_manip_(&localDW->gobj_79);
14770 emxFreeStruct_w_robotics_manip_(&localDW->gobj_78);
14771 emxFreeStruct_w_robotics_manip_(&localDW->gobj_77);
14772 emxFreeStruct_w_robotics_manip_(&localDW->gobj_76);
14773 emxFreeStruct_w_robotics_manip_(&localDW->gobj_75);
14774 emxFreeStruct_w_robotics_manip_(&localDW->gobj_74);
14775 emxFreeStruct_w_robotics_manip_(&localDW->gobj_73);
14776 emxFreeStruct_w_robotics_manip_(&localDW->gobj_72);
14777 emxFreeStruct_w_robotics_manip_(&localDW->gobj_71);
14778 emxFreeStruct_w_robotics_manip_(&localDW->gobj_70);
14779 emxFreeStruct_w_robotics_manip_(&localDW->gobj_69);
14780 emxFreeStruct_w_robotics_manip_(&localDW->gobj_68);
14781 emxFreeStruct_w_robotics_manip_(&localDW->gobj_67);
14782 emxFreeStruct_w_robotics_manip_(&localDW->gobj_66);
14783 emxFreeStruct_w_robotics_manip_(&localDW->gobj_65);
14784 emxFreeStruct_w_robotics_manip_(&localDW->gobj_64);
14785 emxFreeStruct_w_robotics_manip_(&localDW->gobj_63);
14786 emxFreeStruct_w_robotics_manip_(&localDW->gobj_62);
14787 emxFreeStruct_w_robotics_manip_(&localDW->gobj_61);
14788 emxFreeStruct_w_robotics_manip_(&localDW->gobj_60);
14789 emxFreeStruct_w_robotics_manip_(&localDW->gobj_59);
14790 emxFreeStruct_w_robotics_manip_(&localDW->gobj_58);
14791 emxFreeStruct_w_robotics_manip_(&localDW->gobj_57);
14792 emxFreeStruct_w_robotics_manip_(&localDW->gobj_56);
14793 emxFreeStruct_w_robotics_manip_(&localDW->gobj_55);
14794 emxFreeStruct_w_robotics_manip_(&localDW->gobj_54);
14795 emxFreeStruct_w_robotics_manip_(&localDW->gobj_53);
14796 emxFreeStruct_w_robotics_manip_(&localDW->gobj_52);
14797 emxFreeStruct_x_robotics_manip_(&localDW->gobj_83);
14798 emxFreeStruct_x_robotics_manip_(&localDW->gobj_84);
14799 emxFreeStruct_f_robotics_manip_(&localDW->gobj_85);
14800 emxFreeStruct_h_robotics_core_i(&localDW->gobj_86);
14801 emxFreeStruct_h_robotics_core_i(&localDW->gobj_87);
14802 emxFreeStruct_v_robotics_manip_(&localDW->gobj_88);
14803 emxFreeStruct_v_robotics_manip_(&localDW->gobj_103);
14804 emxFreeStruct_v_robotics_manip_(&localDW->gobj_102);
14805 emxFreeStruct_v_robotics_manip_(&localDW->gobj_101);
14806 emxFreeStruct_v_robotics_manip_(&localDW->gobj_100);
14807 emxFreeStruct_v_robotics_manip_(&localDW->gobj_99);
14808 emxFreeStruct_v_robotics_manip_(&localDW->gobj_98);
14809 emxFreeStruct_v_robotics_manip_(&localDW->gobj_97);
14810 emxFreeStruct_v_robotics_manip_(&localDW->gobj_96);
14811 emxFreeStruct_v_robotics_manip_(&localDW->gobj_95);
14812 emxFreeStruct_v_robotics_manip_(&localDW->gobj_94);
14813 emxFreeStruct_v_robotics_manip_(&localDW->gobj_93);
14814 emxFreeStruct_v_robotics_manip_(&localDW->gobj_92);
14815 emxFreeStruct_v_robotics_manip_(&localDW->gobj_91);
14816 emxFreeStruct_v_robotics_manip_(&localDW->gobj_90);
14817 emxFreeStruct_v_robotics_manip_(&localDW->gobj_89);
14818}
14819
14820//
14821// System initialize for atomic system:
14822// synthesized block
14823// synthesized block
14824//
14825void CoordinateTransformationCo_Init(DW_CoordinateTransformationCo_T *localDW)
14826{
14827 // Start for MATLABSystem: '<S12>/Coordinate Transformation Conversion'
14828 localDW->objisempty = true;
14829 localDW->obj.isInitialized = 1;
14830}
14831
14832//
14833// Output and update for atomic system:
14834// synthesized block
14835// synthesized block
14836//
14837void CoordinateTransformationConvers(const real_T rtu_0[4], const real_T rtu_1[3],
14838 B_CoordinateTransformationCon_T *localB)
14839{
14840 int32_T b_k;
14841 real_T u0_idx_2;
14842 real_T u0_idx_3;
14843 int32_T subsa_idx_1;
14844 real_T tempR_tmp;
14845 real_T tempR_tmp_0;
14846 real_T tempR_tmp_1;
14847 real_T tempR_tmp_2;
14848 real_T tempR_tmp_3;
14849
14850 // MATLABSystem: '<S12>/Coordinate Transformation Conversion'
14851 localB->u0_idx_0 = rtu_0[0];
14852 localB->u0_idx_1 = rtu_0[1];
14853 u0_idx_2 = rtu_0[2];
14854 u0_idx_3 = rtu_0[3];
14855 localB->u1[0] = rtu_1[0];
14856 localB->u1[1] = rtu_1[1];
14857 localB->u1[2] = rtu_1[2];
14858 localB->b = 1.0 / sqrt(((localB->u0_idx_0 * localB->u0_idx_0 +
14859 localB->u0_idx_1 * localB->u0_idx_1) + u0_idx_2 * u0_idx_2) + u0_idx_3 *
14860 u0_idx_3);
14861 localB->u0_idx_0 *= localB->b;
14862 localB->u0_idx_1 *= localB->b;
14863 u0_idx_2 *= localB->b;
14864 u0_idx_3 *= localB->b;
14865 localB->b = u0_idx_3 * u0_idx_3;
14866 tempR_tmp_3 = u0_idx_2 * u0_idx_2;
14867 localB->tempR[0] = 1.0 - (tempR_tmp_3 + localB->b) * 2.0;
14868 tempR_tmp = localB->u0_idx_1 * u0_idx_2;
14869 tempR_tmp_0 = localB->u0_idx_0 * u0_idx_3;
14870 localB->tempR[1] = (tempR_tmp - tempR_tmp_0) * 2.0;
14871 tempR_tmp_1 = localB->u0_idx_1 * u0_idx_3;
14872 tempR_tmp_2 = localB->u0_idx_0 * u0_idx_2;
14873 localB->tempR[2] = (tempR_tmp_1 + tempR_tmp_2) * 2.0;
14874 localB->tempR[3] = (tempR_tmp + tempR_tmp_0) * 2.0;
14875 tempR_tmp = localB->u0_idx_1 * localB->u0_idx_1;
14876 localB->tempR[4] = 1.0 - (tempR_tmp + localB->b) * 2.0;
14877 localB->b = u0_idx_2 * u0_idx_3;
14878 tempR_tmp_0 = localB->u0_idx_0 * localB->u0_idx_1;
14879 localB->tempR[5] = (localB->b - tempR_tmp_0) * 2.0;
14880 localB->tempR[6] = (tempR_tmp_1 - tempR_tmp_2) * 2.0;
14881 localB->tempR[7] = (localB->b + tempR_tmp_0) * 2.0;
14882 localB->tempR[8] = 1.0 - (tempR_tmp + tempR_tmp_3) * 2.0;
14883 for (b_k = 0; b_k < 3; b_k++) {
14884 subsa_idx_1 = b_k + 1;
14885 localB->R[subsa_idx_1 - 1] = localB->tempR[(subsa_idx_1 - 1) * 3];
14886 subsa_idx_1 = b_k + 1;
14887 localB->R[subsa_idx_1 + 2] = localB->tempR[(subsa_idx_1 - 1) * 3 + 1];
14888 subsa_idx_1 = b_k + 1;
14889 localB->R[subsa_idx_1 + 5] = localB->tempR[(subsa_idx_1 - 1) * 3 + 2];
14890 }
14891
14892 memset(&localB->CoordinateTransformationConve_g[0], 0, sizeof(real_T) << 4U);
14893 localB->CoordinateTransformationConve_g[15] = 1.0;
14894 for (b_k = 0; b_k < 3; b_k++) {
14895 subsa_idx_1 = b_k << 2;
14896 localB->CoordinateTransformationConve_g[subsa_idx_1] = localB->R[3 * b_k];
14897 localB->CoordinateTransformationConve_g[subsa_idx_1 + 1] = localB->R[3 * b_k
14898 + 1];
14899 localB->CoordinateTransformationConve_g[subsa_idx_1 + 2] = localB->R[3 * b_k
14900 + 2];
14901 localB->CoordinateTransformationConve_g[b_k + 12] = localB->u1[b_k];
14902 }
14903
14904 // End of MATLABSystem: '<S12>/Coordinate Transformation Conversion'
14905}
14906
14907static c_robotics_core_internal_code_T *NameValueParser_NameValueParser
14908 (c_robotics_core_internal_code_T *obj)
14909{
14910 return obj;
14911}
14912
14913static void cartesian__computeProfileParams(real_T i, const real_T wayPoints[12],
14914 const real_T Vel[6], const real_T Acc_data[], real_T *vParam, real_T *aParam,
14915 real_T *tAParam, real_T *tFParam)
14916{
14917 cartesian_trajectory_planner__B.s0_tmp = static_cast<int32_T>(i);
14918 cartesian_trajectory_planner__B.s0_tmp_e =
14919 cartesian_trajectory_planner__B.s0_tmp - 1;
14920 cartesian_trajectory_planner__B.s0 =
14921 wayPoints[cartesian_trajectory_planner__B.s0_tmp_e];
14922 cartesian_trajectory_planner__B.s0_tmp += 5;
14923 cartesian_trajectory_planner__B.sF =
14924 wayPoints[cartesian_trajectory_planner__B.s0_tmp];
14925 cartesian_trajectory_planner__B.deltaSign = 1;
14926 if (wayPoints[cartesian_trajectory_planner__B.s0_tmp] <
14927 wayPoints[cartesian_trajectory_planner__B.s0_tmp_e]) {
14928 cartesian_trajectory_planner__B.s0 =
14929 wayPoints[cartesian_trajectory_planner__B.s0_tmp];
14930 cartesian_trajectory_planner__B.sF =
14931 wayPoints[cartesian_trajectory_planner__B.s0_tmp_e];
14932 cartesian_trajectory_planner__B.deltaSign = -1;
14933 }
14934
14935 *vParam = Vel[cartesian_trajectory_planner__B.s0_tmp_e];
14936 *aParam = Acc_data[cartesian_trajectory_planner__B.s0_tmp_e];
14937 *tAParam = Vel[cartesian_trajectory_planner__B.s0_tmp_e] /
14938 Acc_data[cartesian_trajectory_planner__B.s0_tmp_e];
14939 *tFParam = ((Vel[cartesian_trajectory_planner__B.s0_tmp_e] * *tAParam +
14940 cartesian_trajectory_planner__B.sF) -
14941 cartesian_trajectory_planner__B.s0) /
14942 Vel[cartesian_trajectory_planner__B.s0_tmp_e];
14943 if (cartesian_trajectory_planner__B.s0 == cartesian_trajectory_planner__B.sF)
14944 {
14945 *aParam = 0.0;
14946 *vParam = 0.0;
14947 if (rtIsNaN(*tFParam) || (*tFParam == 0.0)) {
14948 *tFParam = 1.0;
14949 }
14950
14951 *tAParam = *tFParam / 3.0;
14952 }
14953
14954 *vParam *= static_cast<real_T>(cartesian_trajectory_planner__B.deltaSign);
14955 *aParam *= static_cast<real_T>(cartesian_trajectory_planner__B.deltaSign);
14956}
14957
14958static void c_computeScalarLSPBCoefficients(real_T s0, real_T sF, real_T v,
14959 real_T a, real_T ta, real_T tf, real_T coefs[9], real_T breaks[4])
14960{
14961 breaks[0] = 0.0;
14962 breaks[1] = ta;
14963 breaks[2] = tf - ta;
14964 breaks[3] = tf;
14965 memset(&coefs[0], 0, 9U * sizeof(real_T));
14966 if (v == 0.0) {
14967 coefs[6] = s0;
14968 coefs[7] = s0;
14969 coefs[8] = s0;
14970 } else {
14971 coefs[0] = a / 2.0;
14972 coefs[3] = 0.0;
14973 coefs[6] = s0;
14974 coefs[1] = 0.0;
14975 coefs[4] = v;
14976 cartesian_trajectory_planner__B.coefs_tmp = a / 2.0 * (ta * ta);
14977 coefs[7] = cartesian_trajectory_planner__B.coefs_tmp + s0;
14978 coefs[2] = -a / 2.0;
14979 coefs[5] = v;
14980 coefs[8] = (cartesian_trajectory_planner__B.coefs_tmp + sF) - v * ta;
14981 }
14982}
14983
14984static boolean_T cart_checkPolyForMultipleBreaks(const real_T breakMat[24])
14985{
14986 boolean_T hasMultipleBreaks;
14987 boolean_T exitg1;
14988 hasMultipleBreaks = false;
14989 for (cartesian_trajectory_planner__B.b_i_c = 0;
14990 cartesian_trajectory_planner__B.b_i_c < 5;
14991 cartesian_trajectory_planner__B.b_i_c++) {
14992 cartesian_trajectory_planner__B.b_i_f =
14993 cartesian_trajectory_planner__B.b_i_c + 1;
14994 cartesian_trajectory_planner__B.y[0] = fabs
14995 (breakMat[cartesian_trajectory_planner__B.b_i_f - 1] -
14996 breakMat[cartesian_trajectory_planner__B.b_i_c + 1]);
14997 cartesian_trajectory_planner__B.y[1] = fabs
14998 (breakMat[cartesian_trajectory_planner__B.b_i_f + 5] -
14999 breakMat[cartesian_trajectory_planner__B.b_i_c + 7]);
15000 cartesian_trajectory_planner__B.y[2] = fabs
15001 (breakMat[cartesian_trajectory_planner__B.b_i_f + 11] -
15002 breakMat[cartesian_trajectory_planner__B.b_i_c + 13]);
15003 cartesian_trajectory_planner__B.y[3] = fabs
15004 (breakMat[cartesian_trajectory_planner__B.b_i_f + 17] -
15005 breakMat[cartesian_trajectory_planner__B.b_i_c + 19]);
15006 cartesian_trajectory_planner__B.y_l = false;
15007 cartesian_trajectory_planner__B.b_i_f = 0;
15008 exitg1 = false;
15009 while ((!exitg1) && (cartesian_trajectory_planner__B.b_i_f < 4)) {
15010 if (!(cartesian_trajectory_planner__B.y[cartesian_trajectory_planner__B.b_i_f]
15011 > 2.2204460492503131E-16)) {
15012 cartesian_trajectory_planner__B.b_i_f++;
15013 } else {
15014 cartesian_trajectory_planner__B.y_l = true;
15015 exitg1 = true;
15016 }
15017 }
15018
15019 if (cartesian_trajectory_planner__B.y_l || hasMultipleBreaks) {
15020 hasMultipleBreaks = true;
15021 } else {
15022 hasMultipleBreaks = false;
15023 }
15024 }
15025
15026 return hasMultipleBreaks;
15027}
15028
15029static void cartes_processPolynomialResults(const real_T breakMat[24], const
15030 real_T coeffMat[54], boolean_T hasMultipleBreaks,
15031 f_cell_wrap_cartesian_traject_T breaksCell[6], g_cell_wrap_cartesian_traject_T
15032 coeffCell[6])
15033{
15034 boolean_T exitg1;
15035 if (!rtIsNaN(breakMat[18])) {
15036 cartesian_trajectory_planner__B.b_idx_m = 1;
15037 } else {
15038 cartesian_trajectory_planner__B.b_idx_m = 0;
15039 cartesian_trajectory_planner__B.k = 2;
15040 exitg1 = false;
15041 while ((!exitg1) && (cartesian_trajectory_planner__B.k < 7)) {
15042 if (!rtIsNaN(breakMat[cartesian_trajectory_planner__B.k + 17])) {
15043 cartesian_trajectory_planner__B.b_idx_m =
15044 cartesian_trajectory_planner__B.k;
15045 exitg1 = true;
15046 } else {
15047 cartesian_trajectory_planner__B.k++;
15048 }
15049 }
15050 }
15051
15052 if (cartesian_trajectory_planner__B.b_idx_m == 0) {
15053 cartesian_trajectory_planner__B.maxBreaksTime = breakMat[18];
15054 } else {
15055 cartesian_trajectory_planner__B.maxBreaksTime =
15056 breakMat[cartesian_trajectory_planner__B.b_idx_m + 17];
15057 for (cartesian_trajectory_planner__B.k =
15058 cartesian_trajectory_planner__B.b_idx_m + 1;
15059 cartesian_trajectory_planner__B.k < 7;
15060 cartesian_trajectory_planner__B.k++) {
15061 cartesian_trajectory_planner__B.holdTime_o =
15062 breakMat[cartesian_trajectory_planner__B.k + 17];
15063 if (cartesian_trajectory_planner__B.maxBreaksTime <
15064 cartesian_trajectory_planner__B.holdTime_o) {
15065 cartesian_trajectory_planner__B.maxBreaksTime =
15066 cartesian_trajectory_planner__B.holdTime_o;
15067 }
15068 }
15069 }
15070
15071 if (hasMultipleBreaks) {
15072 if (!rtIsNaN(breakMat[0])) {
15073 cartesian_trajectory_planner__B.b_idx_m = 1;
15074 } else {
15075 cartesian_trajectory_planner__B.b_idx_m = 0;
15076 cartesian_trajectory_planner__B.k = 2;
15077 exitg1 = false;
15078 while ((!exitg1) && (cartesian_trajectory_planner__B.k < 5)) {
15079 if (!rtIsNaN(breakMat[(cartesian_trajectory_planner__B.k - 1) * 6])) {
15080 cartesian_trajectory_planner__B.b_idx_m =
15081 cartesian_trajectory_planner__B.k;
15082 exitg1 = true;
15083 } else {
15084 cartesian_trajectory_planner__B.k++;
15085 }
15086 }
15087 }
15088
15089 if (cartesian_trajectory_planner__B.b_idx_m == 0) {
15090 cartesian_trajectory_planner__B.holdTime_o = breakMat[0];
15091 } else {
15092 cartesian_trajectory_planner__B.holdTime_o = breakMat
15093 [(cartesian_trajectory_planner__B.b_idx_m - 1) * 6];
15094 for (cartesian_trajectory_planner__B.k =
15095 cartesian_trajectory_planner__B.b_idx_m + 1;
15096 cartesian_trajectory_planner__B.k < 5;
15097 cartesian_trajectory_planner__B.k++) {
15098 cartesian_trajectory_planner__B.i_m = (cartesian_trajectory_planner__B.k
15099 - 1) * 6;
15100 if (cartesian_trajectory_planner__B.holdTime_o <
15101 breakMat[cartesian_trajectory_planner__B.i_m]) {
15102 cartesian_trajectory_planner__B.holdTime_o =
15103 breakMat[cartesian_trajectory_planner__B.i_m];
15104 }
15105 }
15106 }
15107
15108 if (cartesian_trajectory_planner__B.holdTime_o <
15109 cartesian_trajectory_planner__B.maxBreaksTime) {
15110 breaksCell[0].f1.size[0] = 1;
15111 breaksCell[0].f1.size[1] = 5;
15112 breaksCell[0].f1.data[0] = breakMat[0];
15113 breaksCell[0].f1.data[1] = breakMat[6];
15114 breaksCell[0].f1.data[2] = breakMat[12];
15115 breaksCell[0].f1.data[3] = breakMat[18];
15116 breaksCell[0].f1.data[4] = cartesian_trajectory_planner__B.maxBreaksTime;
15117 cartesian_trajectory_planner__B.holdTime_o = breakMat[18] - breakMat[12];
15118 cartesian_trajectory_planner__B.k = 4;
15119 memset(&cartesian_trajectory_planner__B.coefs_data[0], 0, 12U * sizeof
15120 (real_T));
15121 cartesian_trajectory_planner__B.holdTime[0] =
15122 cartesian_trajectory_planner__B.holdTime_o *
15123 cartesian_trajectory_planner__B.holdTime_o;
15124 cartesian_trajectory_planner__B.holdTime[1] =
15125 cartesian_trajectory_planner__B.holdTime_o;
15126 cartesian_trajectory_planner__B.holdTime[2] = 1.0;
15127 cartesian_trajectory_planner__B.holdTime_o = 0.0;
15128 for (cartesian_trajectory_planner__B.i_m = 0;
15129 cartesian_trajectory_planner__B.i_m < 3;
15130 cartesian_trajectory_planner__B.i_m++) {
15131 cartesian_trajectory_planner__B.b_idx_m =
15132 cartesian_trajectory_planner__B.i_m << 2;
15133 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m]
15134 = coeffMat[18 * cartesian_trajectory_planner__B.i_m];
15135 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m
15136 + 1] = coeffMat[18 * cartesian_trajectory_planner__B.i_m + 6];
15137 cartesian_trajectory_planner__B.coefs_data_tmp = 18 *
15138 cartesian_trajectory_planner__B.i_m + 12;
15139 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m
15140 + 2] = coeffMat[cartesian_trajectory_planner__B.coefs_data_tmp];
15141 cartesian_trajectory_planner__B.holdTime_o +=
15142 coeffMat[cartesian_trajectory_planner__B.coefs_data_tmp] *
15143 cartesian_trajectory_planner__B.holdTime[cartesian_trajectory_planner__B.i_m];
15144 }
15145
15146 cartesian_trajectory_planner__B.coefs_data[11] =
15147 cartesian_trajectory_planner__B.holdTime_o;
15148 } else {
15149 cartesian_trajectory_planner__B.k = 3;
15150 for (cartesian_trajectory_planner__B.i_m = 0;
15151 cartesian_trajectory_planner__B.i_m < 3;
15152 cartesian_trajectory_planner__B.i_m++) {
15153 cartesian_trajectory_planner__B.coefs_data[3 *
15154 cartesian_trajectory_planner__B.i_m] = coeffMat[18 *
15155 cartesian_trajectory_planner__B.i_m];
15156 cartesian_trajectory_planner__B.coefs_data[3 *
15157 cartesian_trajectory_planner__B.i_m + 1] = coeffMat[18 *
15158 cartesian_trajectory_planner__B.i_m + 6];
15159 cartesian_trajectory_planner__B.coefs_data[3 *
15160 cartesian_trajectory_planner__B.i_m + 2] = coeffMat[18 *
15161 cartesian_trajectory_planner__B.i_m + 12];
15162 }
15163
15164 breaksCell[0].f1.size[0] = 1;
15165 breaksCell[0].f1.size[1] = 4;
15166 breaksCell[0].f1.data[0] = breakMat[0];
15167 breaksCell[0].f1.data[1] = breakMat[6];
15168 breaksCell[0].f1.data[2] = breakMat[12];
15169 breaksCell[0].f1.data[3] = breakMat[18];
15170 }
15171
15172 coeffCell[0].f1.size[0] = cartesian_trajectory_planner__B.k;
15173 coeffCell[0].f1.size[1] = 3;
15174 cartesian_trajectory_planner__B.k = cartesian_trajectory_planner__B.k * 3 -
15175 1;
15176 for (cartesian_trajectory_planner__B.i_m = 0;
15177 cartesian_trajectory_planner__B.i_m <=
15178 cartesian_trajectory_planner__B.k; cartesian_trajectory_planner__B.i_m
15179 ++) {
15180 coeffCell[0].f1.data[cartesian_trajectory_planner__B.i_m] =
15181 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.i_m];
15182 }
15183
15184 if (!rtIsNaN(breakMat[1])) {
15185 cartesian_trajectory_planner__B.b_idx_m = 1;
15186 } else {
15187 cartesian_trajectory_planner__B.b_idx_m = 0;
15188 cartesian_trajectory_planner__B.k = 2;
15189 exitg1 = false;
15190 while ((!exitg1) && (cartesian_trajectory_planner__B.k < 5)) {
15191 if (!rtIsNaN(breakMat[(cartesian_trajectory_planner__B.k - 1) * 6 + 1]))
15192 {
15193 cartesian_trajectory_planner__B.b_idx_m =
15194 cartesian_trajectory_planner__B.k;
15195 exitg1 = true;
15196 } else {
15197 cartesian_trajectory_planner__B.k++;
15198 }
15199 }
15200 }
15201
15202 if (cartesian_trajectory_planner__B.b_idx_m == 0) {
15203 cartesian_trajectory_planner__B.holdTime_o = breakMat[1];
15204 } else {
15205 cartesian_trajectory_planner__B.holdTime_o = breakMat
15206 [(cartesian_trajectory_planner__B.b_idx_m - 1) * 6 + 1];
15207 for (cartesian_trajectory_planner__B.k =
15208 cartesian_trajectory_planner__B.b_idx_m + 1;
15209 cartesian_trajectory_planner__B.k < 5;
15210 cartesian_trajectory_planner__B.k++) {
15211 cartesian_trajectory_planner__B.i_m = (cartesian_trajectory_planner__B.k
15212 - 1) * 6 + 1;
15213 if (cartesian_trajectory_planner__B.holdTime_o <
15214 breakMat[cartesian_trajectory_planner__B.i_m]) {
15215 cartesian_trajectory_planner__B.holdTime_o =
15216 breakMat[cartesian_trajectory_planner__B.i_m];
15217 }
15218 }
15219 }
15220
15221 if (cartesian_trajectory_planner__B.holdTime_o <
15222 cartesian_trajectory_planner__B.maxBreaksTime) {
15223 breaksCell[1].f1.size[0] = 1;
15224 breaksCell[1].f1.size[1] = 5;
15225 breaksCell[1].f1.data[0] = breakMat[1];
15226 breaksCell[1].f1.data[1] = breakMat[7];
15227 breaksCell[1].f1.data[2] = breakMat[13];
15228 breaksCell[1].f1.data[3] = breakMat[19];
15229 breaksCell[1].f1.data[4] = cartesian_trajectory_planner__B.maxBreaksTime;
15230 cartesian_trajectory_planner__B.holdTime_o = breakMat[19] - breakMat[13];
15231 cartesian_trajectory_planner__B.k = 4;
15232 memset(&cartesian_trajectory_planner__B.coefs_data[0], 0, 12U * sizeof
15233 (real_T));
15234 cartesian_trajectory_planner__B.holdTime[0] =
15235 cartesian_trajectory_planner__B.holdTime_o *
15236 cartesian_trajectory_planner__B.holdTime_o;
15237 cartesian_trajectory_planner__B.holdTime[1] =
15238 cartesian_trajectory_planner__B.holdTime_o;
15239 cartesian_trajectory_planner__B.holdTime[2] = 1.0;
15240 cartesian_trajectory_planner__B.holdTime_o = 0.0;
15241 for (cartesian_trajectory_planner__B.i_m = 0;
15242 cartesian_trajectory_planner__B.i_m < 3;
15243 cartesian_trajectory_planner__B.i_m++) {
15244 cartesian_trajectory_planner__B.b_idx_m =
15245 cartesian_trajectory_planner__B.i_m << 2;
15246 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m]
15247 = coeffMat[18 * cartesian_trajectory_planner__B.i_m + 1];
15248 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m
15249 + 1] = coeffMat[18 * cartesian_trajectory_planner__B.i_m + 7];
15250 cartesian_trajectory_planner__B.coefs_data_tmp = 18 *
15251 cartesian_trajectory_planner__B.i_m + 13;
15252 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m
15253 + 2] = coeffMat[cartesian_trajectory_planner__B.coefs_data_tmp];
15254 cartesian_trajectory_planner__B.holdTime_o +=
15255 coeffMat[cartesian_trajectory_planner__B.coefs_data_tmp] *
15256 cartesian_trajectory_planner__B.holdTime[cartesian_trajectory_planner__B.i_m];
15257 }
15258
15259 cartesian_trajectory_planner__B.coefs_data[11] =
15260 cartesian_trajectory_planner__B.holdTime_o;
15261 } else {
15262 cartesian_trajectory_planner__B.k = 3;
15263 for (cartesian_trajectory_planner__B.i_m = 0;
15264 cartesian_trajectory_planner__B.i_m < 3;
15265 cartesian_trajectory_planner__B.i_m++) {
15266 cartesian_trajectory_planner__B.coefs_data[3 *
15267 cartesian_trajectory_planner__B.i_m] = coeffMat[18 *
15268 cartesian_trajectory_planner__B.i_m + 1];
15269 cartesian_trajectory_planner__B.coefs_data[3 *
15270 cartesian_trajectory_planner__B.i_m + 1] = coeffMat[18 *
15271 cartesian_trajectory_planner__B.i_m + 7];
15272 cartesian_trajectory_planner__B.coefs_data[3 *
15273 cartesian_trajectory_planner__B.i_m + 2] = coeffMat[18 *
15274 cartesian_trajectory_planner__B.i_m + 13];
15275 }
15276
15277 breaksCell[1].f1.size[0] = 1;
15278 breaksCell[1].f1.size[1] = 4;
15279 breaksCell[1].f1.data[0] = breakMat[1];
15280 breaksCell[1].f1.data[1] = breakMat[7];
15281 breaksCell[1].f1.data[2] = breakMat[13];
15282 breaksCell[1].f1.data[3] = breakMat[19];
15283 }
15284
15285 coeffCell[1].f1.size[0] = cartesian_trajectory_planner__B.k;
15286 coeffCell[1].f1.size[1] = 3;
15287 cartesian_trajectory_planner__B.k = cartesian_trajectory_planner__B.k * 3 -
15288 1;
15289 for (cartesian_trajectory_planner__B.i_m = 0;
15290 cartesian_trajectory_planner__B.i_m <=
15291 cartesian_trajectory_planner__B.k; cartesian_trajectory_planner__B.i_m
15292 ++) {
15293 coeffCell[1].f1.data[cartesian_trajectory_planner__B.i_m] =
15294 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.i_m];
15295 }
15296
15297 if (!rtIsNaN(breakMat[2])) {
15298 cartesian_trajectory_planner__B.b_idx_m = 1;
15299 } else {
15300 cartesian_trajectory_planner__B.b_idx_m = 0;
15301 cartesian_trajectory_planner__B.k = 2;
15302 exitg1 = false;
15303 while ((!exitg1) && (cartesian_trajectory_planner__B.k < 5)) {
15304 if (!rtIsNaN(breakMat[(cartesian_trajectory_planner__B.k - 1) * 6 + 2]))
15305 {
15306 cartesian_trajectory_planner__B.b_idx_m =
15307 cartesian_trajectory_planner__B.k;
15308 exitg1 = true;
15309 } else {
15310 cartesian_trajectory_planner__B.k++;
15311 }
15312 }
15313 }
15314
15315 if (cartesian_trajectory_planner__B.b_idx_m == 0) {
15316 cartesian_trajectory_planner__B.holdTime_o = breakMat[2];
15317 } else {
15318 cartesian_trajectory_planner__B.holdTime_o = breakMat
15319 [(cartesian_trajectory_planner__B.b_idx_m - 1) * 6 + 2];
15320 for (cartesian_trajectory_planner__B.k =
15321 cartesian_trajectory_planner__B.b_idx_m + 1;
15322 cartesian_trajectory_planner__B.k < 5;
15323 cartesian_trajectory_planner__B.k++) {
15324 cartesian_trajectory_planner__B.i_m = (cartesian_trajectory_planner__B.k
15325 - 1) * 6 + 2;
15326 if (cartesian_trajectory_planner__B.holdTime_o <
15327 breakMat[cartesian_trajectory_planner__B.i_m]) {
15328 cartesian_trajectory_planner__B.holdTime_o =
15329 breakMat[cartesian_trajectory_planner__B.i_m];
15330 }
15331 }
15332 }
15333
15334 if (cartesian_trajectory_planner__B.holdTime_o <
15335 cartesian_trajectory_planner__B.maxBreaksTime) {
15336 breaksCell[2].f1.size[0] = 1;
15337 breaksCell[2].f1.size[1] = 5;
15338 breaksCell[2].f1.data[0] = breakMat[2];
15339 breaksCell[2].f1.data[1] = breakMat[8];
15340 breaksCell[2].f1.data[2] = breakMat[14];
15341 breaksCell[2].f1.data[3] = breakMat[20];
15342 breaksCell[2].f1.data[4] = cartesian_trajectory_planner__B.maxBreaksTime;
15343 cartesian_trajectory_planner__B.holdTime_o = breakMat[20] - breakMat[14];
15344 cartesian_trajectory_planner__B.k = 4;
15345 memset(&cartesian_trajectory_planner__B.coefs_data[0], 0, 12U * sizeof
15346 (real_T));
15347 cartesian_trajectory_planner__B.holdTime[0] =
15348 cartesian_trajectory_planner__B.holdTime_o *
15349 cartesian_trajectory_planner__B.holdTime_o;
15350 cartesian_trajectory_planner__B.holdTime[1] =
15351 cartesian_trajectory_planner__B.holdTime_o;
15352 cartesian_trajectory_planner__B.holdTime[2] = 1.0;
15353 cartesian_trajectory_planner__B.holdTime_o = 0.0;
15354 for (cartesian_trajectory_planner__B.i_m = 0;
15355 cartesian_trajectory_planner__B.i_m < 3;
15356 cartesian_trajectory_planner__B.i_m++) {
15357 cartesian_trajectory_planner__B.b_idx_m =
15358 cartesian_trajectory_planner__B.i_m << 2;
15359 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m]
15360 = coeffMat[18 * cartesian_trajectory_planner__B.i_m + 2];
15361 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m
15362 + 1] = coeffMat[18 * cartesian_trajectory_planner__B.i_m + 8];
15363 cartesian_trajectory_planner__B.coefs_data_tmp = 18 *
15364 cartesian_trajectory_planner__B.i_m + 14;
15365 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m
15366 + 2] = coeffMat[cartesian_trajectory_planner__B.coefs_data_tmp];
15367 cartesian_trajectory_planner__B.holdTime_o +=
15368 coeffMat[cartesian_trajectory_planner__B.coefs_data_tmp] *
15369 cartesian_trajectory_planner__B.holdTime[cartesian_trajectory_planner__B.i_m];
15370 }
15371
15372 cartesian_trajectory_planner__B.coefs_data[11] =
15373 cartesian_trajectory_planner__B.holdTime_o;
15374 } else {
15375 cartesian_trajectory_planner__B.k = 3;
15376 for (cartesian_trajectory_planner__B.i_m = 0;
15377 cartesian_trajectory_planner__B.i_m < 3;
15378 cartesian_trajectory_planner__B.i_m++) {
15379 cartesian_trajectory_planner__B.coefs_data[3 *
15380 cartesian_trajectory_planner__B.i_m] = coeffMat[18 *
15381 cartesian_trajectory_planner__B.i_m + 2];
15382 cartesian_trajectory_planner__B.coefs_data[3 *
15383 cartesian_trajectory_planner__B.i_m + 1] = coeffMat[18 *
15384 cartesian_trajectory_planner__B.i_m + 8];
15385 cartesian_trajectory_planner__B.coefs_data[3 *
15386 cartesian_trajectory_planner__B.i_m + 2] = coeffMat[18 *
15387 cartesian_trajectory_planner__B.i_m + 14];
15388 }
15389
15390 breaksCell[2].f1.size[0] = 1;
15391 breaksCell[2].f1.size[1] = 4;
15392 breaksCell[2].f1.data[0] = breakMat[2];
15393 breaksCell[2].f1.data[1] = breakMat[8];
15394 breaksCell[2].f1.data[2] = breakMat[14];
15395 breaksCell[2].f1.data[3] = breakMat[20];
15396 }
15397
15398 coeffCell[2].f1.size[0] = cartesian_trajectory_planner__B.k;
15399 coeffCell[2].f1.size[1] = 3;
15400 cartesian_trajectory_planner__B.k = cartesian_trajectory_planner__B.k * 3 -
15401 1;
15402 for (cartesian_trajectory_planner__B.i_m = 0;
15403 cartesian_trajectory_planner__B.i_m <=
15404 cartesian_trajectory_planner__B.k; cartesian_trajectory_planner__B.i_m
15405 ++) {
15406 coeffCell[2].f1.data[cartesian_trajectory_planner__B.i_m] =
15407 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.i_m];
15408 }
15409
15410 if (!rtIsNaN(breakMat[3])) {
15411 cartesian_trajectory_planner__B.b_idx_m = 1;
15412 } else {
15413 cartesian_trajectory_planner__B.b_idx_m = 0;
15414 cartesian_trajectory_planner__B.k = 2;
15415 exitg1 = false;
15416 while ((!exitg1) && (cartesian_trajectory_planner__B.k < 5)) {
15417 if (!rtIsNaN(breakMat[(cartesian_trajectory_planner__B.k - 1) * 6 + 3]))
15418 {
15419 cartesian_trajectory_planner__B.b_idx_m =
15420 cartesian_trajectory_planner__B.k;
15421 exitg1 = true;
15422 } else {
15423 cartesian_trajectory_planner__B.k++;
15424 }
15425 }
15426 }
15427
15428 if (cartesian_trajectory_planner__B.b_idx_m == 0) {
15429 cartesian_trajectory_planner__B.holdTime_o = breakMat[3];
15430 } else {
15431 cartesian_trajectory_planner__B.holdTime_o = breakMat
15432 [(cartesian_trajectory_planner__B.b_idx_m - 1) * 6 + 3];
15433 for (cartesian_trajectory_planner__B.k =
15434 cartesian_trajectory_planner__B.b_idx_m + 1;
15435 cartesian_trajectory_planner__B.k < 5;
15436 cartesian_trajectory_planner__B.k++) {
15437 cartesian_trajectory_planner__B.i_m = (cartesian_trajectory_planner__B.k
15438 - 1) * 6 + 3;
15439 if (cartesian_trajectory_planner__B.holdTime_o <
15440 breakMat[cartesian_trajectory_planner__B.i_m]) {
15441 cartesian_trajectory_planner__B.holdTime_o =
15442 breakMat[cartesian_trajectory_planner__B.i_m];
15443 }
15444 }
15445 }
15446
15447 if (cartesian_trajectory_planner__B.holdTime_o <
15448 cartesian_trajectory_planner__B.maxBreaksTime) {
15449 breaksCell[3].f1.size[0] = 1;
15450 breaksCell[3].f1.size[1] = 5;
15451 breaksCell[3].f1.data[0] = breakMat[3];
15452 breaksCell[3].f1.data[1] = breakMat[9];
15453 breaksCell[3].f1.data[2] = breakMat[15];
15454 breaksCell[3].f1.data[3] = breakMat[21];
15455 breaksCell[3].f1.data[4] = cartesian_trajectory_planner__B.maxBreaksTime;
15456 cartesian_trajectory_planner__B.holdTime_o = breakMat[21] - breakMat[15];
15457 cartesian_trajectory_planner__B.k = 4;
15458 memset(&cartesian_trajectory_planner__B.coefs_data[0], 0, 12U * sizeof
15459 (real_T));
15460 cartesian_trajectory_planner__B.holdTime[0] =
15461 cartesian_trajectory_planner__B.holdTime_o *
15462 cartesian_trajectory_planner__B.holdTime_o;
15463 cartesian_trajectory_planner__B.holdTime[1] =
15464 cartesian_trajectory_planner__B.holdTime_o;
15465 cartesian_trajectory_planner__B.holdTime[2] = 1.0;
15466 cartesian_trajectory_planner__B.holdTime_o = 0.0;
15467 for (cartesian_trajectory_planner__B.i_m = 0;
15468 cartesian_trajectory_planner__B.i_m < 3;
15469 cartesian_trajectory_planner__B.i_m++) {
15470 cartesian_trajectory_planner__B.b_idx_m =
15471 cartesian_trajectory_planner__B.i_m << 2;
15472 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m]
15473 = coeffMat[18 * cartesian_trajectory_planner__B.i_m + 3];
15474 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m
15475 + 1] = coeffMat[18 * cartesian_trajectory_planner__B.i_m + 9];
15476 cartesian_trajectory_planner__B.coefs_data_tmp = 18 *
15477 cartesian_trajectory_planner__B.i_m + 15;
15478 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m
15479 + 2] = coeffMat[cartesian_trajectory_planner__B.coefs_data_tmp];
15480 cartesian_trajectory_planner__B.holdTime_o +=
15481 coeffMat[cartesian_trajectory_planner__B.coefs_data_tmp] *
15482 cartesian_trajectory_planner__B.holdTime[cartesian_trajectory_planner__B.i_m];
15483 }
15484
15485 cartesian_trajectory_planner__B.coefs_data[11] =
15486 cartesian_trajectory_planner__B.holdTime_o;
15487 } else {
15488 cartesian_trajectory_planner__B.k = 3;
15489 for (cartesian_trajectory_planner__B.i_m = 0;
15490 cartesian_trajectory_planner__B.i_m < 3;
15491 cartesian_trajectory_planner__B.i_m++) {
15492 cartesian_trajectory_planner__B.coefs_data[3 *
15493 cartesian_trajectory_planner__B.i_m] = coeffMat[18 *
15494 cartesian_trajectory_planner__B.i_m + 3];
15495 cartesian_trajectory_planner__B.coefs_data[3 *
15496 cartesian_trajectory_planner__B.i_m + 1] = coeffMat[18 *
15497 cartesian_trajectory_planner__B.i_m + 9];
15498 cartesian_trajectory_planner__B.coefs_data[3 *
15499 cartesian_trajectory_planner__B.i_m + 2] = coeffMat[18 *
15500 cartesian_trajectory_planner__B.i_m + 15];
15501 }
15502
15503 breaksCell[3].f1.size[0] = 1;
15504 breaksCell[3].f1.size[1] = 4;
15505 breaksCell[3].f1.data[0] = breakMat[3];
15506 breaksCell[3].f1.data[1] = breakMat[9];
15507 breaksCell[3].f1.data[2] = breakMat[15];
15508 breaksCell[3].f1.data[3] = breakMat[21];
15509 }
15510
15511 coeffCell[3].f1.size[0] = cartesian_trajectory_planner__B.k;
15512 coeffCell[3].f1.size[1] = 3;
15513 cartesian_trajectory_planner__B.k = cartesian_trajectory_planner__B.k * 3 -
15514 1;
15515 for (cartesian_trajectory_planner__B.i_m = 0;
15516 cartesian_trajectory_planner__B.i_m <=
15517 cartesian_trajectory_planner__B.k; cartesian_trajectory_planner__B.i_m
15518 ++) {
15519 coeffCell[3].f1.data[cartesian_trajectory_planner__B.i_m] =
15520 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.i_m];
15521 }
15522
15523 if (!rtIsNaN(breakMat[4])) {
15524 cartesian_trajectory_planner__B.b_idx_m = 1;
15525 } else {
15526 cartesian_trajectory_planner__B.b_idx_m = 0;
15527 cartesian_trajectory_planner__B.k = 2;
15528 exitg1 = false;
15529 while ((!exitg1) && (cartesian_trajectory_planner__B.k < 5)) {
15530 if (!rtIsNaN(breakMat[(cartesian_trajectory_planner__B.k - 1) * 6 + 4]))
15531 {
15532 cartesian_trajectory_planner__B.b_idx_m =
15533 cartesian_trajectory_planner__B.k;
15534 exitg1 = true;
15535 } else {
15536 cartesian_trajectory_planner__B.k++;
15537 }
15538 }
15539 }
15540
15541 if (cartesian_trajectory_planner__B.b_idx_m == 0) {
15542 cartesian_trajectory_planner__B.holdTime_o = breakMat[4];
15543 } else {
15544 cartesian_trajectory_planner__B.holdTime_o = breakMat
15545 [(cartesian_trajectory_planner__B.b_idx_m - 1) * 6 + 4];
15546 for (cartesian_trajectory_planner__B.k =
15547 cartesian_trajectory_planner__B.b_idx_m + 1;
15548 cartesian_trajectory_planner__B.k < 5;
15549 cartesian_trajectory_planner__B.k++) {
15550 cartesian_trajectory_planner__B.i_m = (cartesian_trajectory_planner__B.k
15551 - 1) * 6 + 4;
15552 if (cartesian_trajectory_planner__B.holdTime_o <
15553 breakMat[cartesian_trajectory_planner__B.i_m]) {
15554 cartesian_trajectory_planner__B.holdTime_o =
15555 breakMat[cartesian_trajectory_planner__B.i_m];
15556 }
15557 }
15558 }
15559
15560 if (cartesian_trajectory_planner__B.holdTime_o <
15561 cartesian_trajectory_planner__B.maxBreaksTime) {
15562 breaksCell[4].f1.size[0] = 1;
15563 breaksCell[4].f1.size[1] = 5;
15564 breaksCell[4].f1.data[0] = breakMat[4];
15565 breaksCell[4].f1.data[1] = breakMat[10];
15566 breaksCell[4].f1.data[2] = breakMat[16];
15567 breaksCell[4].f1.data[3] = breakMat[22];
15568 breaksCell[4].f1.data[4] = cartesian_trajectory_planner__B.maxBreaksTime;
15569 cartesian_trajectory_planner__B.holdTime_o = breakMat[22] - breakMat[16];
15570 cartesian_trajectory_planner__B.k = 4;
15571 memset(&cartesian_trajectory_planner__B.coefs_data[0], 0, 12U * sizeof
15572 (real_T));
15573 cartesian_trajectory_planner__B.holdTime[0] =
15574 cartesian_trajectory_planner__B.holdTime_o *
15575 cartesian_trajectory_planner__B.holdTime_o;
15576 cartesian_trajectory_planner__B.holdTime[1] =
15577 cartesian_trajectory_planner__B.holdTime_o;
15578 cartesian_trajectory_planner__B.holdTime[2] = 1.0;
15579 cartesian_trajectory_planner__B.holdTime_o = 0.0;
15580 for (cartesian_trajectory_planner__B.i_m = 0;
15581 cartesian_trajectory_planner__B.i_m < 3;
15582 cartesian_trajectory_planner__B.i_m++) {
15583 cartesian_trajectory_planner__B.b_idx_m =
15584 cartesian_trajectory_planner__B.i_m << 2;
15585 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m]
15586 = coeffMat[18 * cartesian_trajectory_planner__B.i_m + 4];
15587 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m
15588 + 1] = coeffMat[18 * cartesian_trajectory_planner__B.i_m + 10];
15589 cartesian_trajectory_planner__B.coefs_data_tmp = 18 *
15590 cartesian_trajectory_planner__B.i_m + 16;
15591 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m
15592 + 2] = coeffMat[cartesian_trajectory_planner__B.coefs_data_tmp];
15593 cartesian_trajectory_planner__B.holdTime_o +=
15594 coeffMat[cartesian_trajectory_planner__B.coefs_data_tmp] *
15595 cartesian_trajectory_planner__B.holdTime[cartesian_trajectory_planner__B.i_m];
15596 }
15597
15598 cartesian_trajectory_planner__B.coefs_data[11] =
15599 cartesian_trajectory_planner__B.holdTime_o;
15600 } else {
15601 cartesian_trajectory_planner__B.k = 3;
15602 for (cartesian_trajectory_planner__B.i_m = 0;
15603 cartesian_trajectory_planner__B.i_m < 3;
15604 cartesian_trajectory_planner__B.i_m++) {
15605 cartesian_trajectory_planner__B.coefs_data[3 *
15606 cartesian_trajectory_planner__B.i_m] = coeffMat[18 *
15607 cartesian_trajectory_planner__B.i_m + 4];
15608 cartesian_trajectory_planner__B.coefs_data[3 *
15609 cartesian_trajectory_planner__B.i_m + 1] = coeffMat[18 *
15610 cartesian_trajectory_planner__B.i_m + 10];
15611 cartesian_trajectory_planner__B.coefs_data[3 *
15612 cartesian_trajectory_planner__B.i_m + 2] = coeffMat[18 *
15613 cartesian_trajectory_planner__B.i_m + 16];
15614 }
15615
15616 breaksCell[4].f1.size[0] = 1;
15617 breaksCell[4].f1.size[1] = 4;
15618 breaksCell[4].f1.data[0] = breakMat[4];
15619 breaksCell[4].f1.data[1] = breakMat[10];
15620 breaksCell[4].f1.data[2] = breakMat[16];
15621 breaksCell[4].f1.data[3] = breakMat[22];
15622 }
15623
15624 coeffCell[4].f1.size[0] = cartesian_trajectory_planner__B.k;
15625 coeffCell[4].f1.size[1] = 3;
15626 cartesian_trajectory_planner__B.k = cartesian_trajectory_planner__B.k * 3 -
15627 1;
15628 for (cartesian_trajectory_planner__B.i_m = 0;
15629 cartesian_trajectory_planner__B.i_m <=
15630 cartesian_trajectory_planner__B.k; cartesian_trajectory_planner__B.i_m
15631 ++) {
15632 coeffCell[4].f1.data[cartesian_trajectory_planner__B.i_m] =
15633 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.i_m];
15634 }
15635
15636 if (!rtIsNaN(breakMat[5])) {
15637 cartesian_trajectory_planner__B.b_idx_m = 1;
15638 } else {
15639 cartesian_trajectory_planner__B.b_idx_m = 0;
15640 cartesian_trajectory_planner__B.k = 2;
15641 exitg1 = false;
15642 while ((!exitg1) && (cartesian_trajectory_planner__B.k < 5)) {
15643 if (!rtIsNaN(breakMat[(cartesian_trajectory_planner__B.k - 1) * 6 + 5]))
15644 {
15645 cartesian_trajectory_planner__B.b_idx_m =
15646 cartesian_trajectory_planner__B.k;
15647 exitg1 = true;
15648 } else {
15649 cartesian_trajectory_planner__B.k++;
15650 }
15651 }
15652 }
15653
15654 if (cartesian_trajectory_planner__B.b_idx_m == 0) {
15655 cartesian_trajectory_planner__B.holdTime_o = breakMat[5];
15656 } else {
15657 cartesian_trajectory_planner__B.holdTime_o = breakMat
15658 [(cartesian_trajectory_planner__B.b_idx_m - 1) * 6 + 5];
15659 for (cartesian_trajectory_planner__B.k =
15660 cartesian_trajectory_planner__B.b_idx_m + 1;
15661 cartesian_trajectory_planner__B.k < 5;
15662 cartesian_trajectory_planner__B.k++) {
15663 cartesian_trajectory_planner__B.i_m = (cartesian_trajectory_planner__B.k
15664 - 1) * 6 + 5;
15665 if (cartesian_trajectory_planner__B.holdTime_o <
15666 breakMat[cartesian_trajectory_planner__B.i_m]) {
15667 cartesian_trajectory_planner__B.holdTime_o =
15668 breakMat[cartesian_trajectory_planner__B.i_m];
15669 }
15670 }
15671 }
15672
15673 if (cartesian_trajectory_planner__B.holdTime_o <
15674 cartesian_trajectory_planner__B.maxBreaksTime) {
15675 breaksCell[5].f1.size[0] = 1;
15676 breaksCell[5].f1.size[1] = 5;
15677 breaksCell[5].f1.data[0] = breakMat[5];
15678 breaksCell[5].f1.data[1] = breakMat[11];
15679 breaksCell[5].f1.data[2] = breakMat[17];
15680 breaksCell[5].f1.data[3] = breakMat[23];
15681 breaksCell[5].f1.data[4] = cartesian_trajectory_planner__B.maxBreaksTime;
15682 cartesian_trajectory_planner__B.holdTime_o = breakMat[23] - breakMat[17];
15683 cartesian_trajectory_planner__B.k = 4;
15684 memset(&cartesian_trajectory_planner__B.coefs_data[0], 0, 12U * sizeof
15685 (real_T));
15686 cartesian_trajectory_planner__B.holdTime[0] =
15687 cartesian_trajectory_planner__B.holdTime_o *
15688 cartesian_trajectory_planner__B.holdTime_o;
15689 cartesian_trajectory_planner__B.holdTime[1] =
15690 cartesian_trajectory_planner__B.holdTime_o;
15691 cartesian_trajectory_planner__B.holdTime[2] = 1.0;
15692 cartesian_trajectory_planner__B.holdTime_o = 0.0;
15693 for (cartesian_trajectory_planner__B.i_m = 0;
15694 cartesian_trajectory_planner__B.i_m < 3;
15695 cartesian_trajectory_planner__B.i_m++) {
15696 cartesian_trajectory_planner__B.b_idx_m =
15697 cartesian_trajectory_planner__B.i_m << 2;
15698 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m]
15699 = coeffMat[18 * cartesian_trajectory_planner__B.i_m + 5];
15700 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m
15701 + 1] = coeffMat[18 * cartesian_trajectory_planner__B.i_m + 11];
15702 cartesian_trajectory_planner__B.coefs_data_tmp = 18 *
15703 cartesian_trajectory_planner__B.i_m + 17;
15704 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.b_idx_m
15705 + 2] = coeffMat[cartesian_trajectory_planner__B.coefs_data_tmp];
15706 cartesian_trajectory_planner__B.holdTime_o +=
15707 coeffMat[cartesian_trajectory_planner__B.coefs_data_tmp] *
15708 cartesian_trajectory_planner__B.holdTime[cartesian_trajectory_planner__B.i_m];
15709 }
15710
15711 cartesian_trajectory_planner__B.coefs_data[11] =
15712 cartesian_trajectory_planner__B.holdTime_o;
15713 } else {
15714 cartesian_trajectory_planner__B.k = 3;
15715 for (cartesian_trajectory_planner__B.i_m = 0;
15716 cartesian_trajectory_planner__B.i_m < 3;
15717 cartesian_trajectory_planner__B.i_m++) {
15718 cartesian_trajectory_planner__B.coefs_data[3 *
15719 cartesian_trajectory_planner__B.i_m] = coeffMat[18 *
15720 cartesian_trajectory_planner__B.i_m + 5];
15721 cartesian_trajectory_planner__B.coefs_data[3 *
15722 cartesian_trajectory_planner__B.i_m + 1] = coeffMat[18 *
15723 cartesian_trajectory_planner__B.i_m + 11];
15724 cartesian_trajectory_planner__B.coefs_data[3 *
15725 cartesian_trajectory_planner__B.i_m + 2] = coeffMat[18 *
15726 cartesian_trajectory_planner__B.i_m + 17];
15727 }
15728
15729 breaksCell[5].f1.size[0] = 1;
15730 breaksCell[5].f1.size[1] = 4;
15731 breaksCell[5].f1.data[0] = breakMat[5];
15732 breaksCell[5].f1.data[1] = breakMat[11];
15733 breaksCell[5].f1.data[2] = breakMat[17];
15734 breaksCell[5].f1.data[3] = breakMat[23];
15735 }
15736
15737 coeffCell[5].f1.size[0] = cartesian_trajectory_planner__B.k;
15738 coeffCell[5].f1.size[1] = 3;
15739 cartesian_trajectory_planner__B.k = cartesian_trajectory_planner__B.k * 3 -
15740 1;
15741 for (cartesian_trajectory_planner__B.i_m = 0;
15742 cartesian_trajectory_planner__B.i_m <=
15743 cartesian_trajectory_planner__B.k; cartesian_trajectory_planner__B.i_m
15744 ++) {
15745 coeffCell[5].f1.data[cartesian_trajectory_planner__B.i_m] =
15746 cartesian_trajectory_planner__B.coefs_data[cartesian_trajectory_planner__B.i_m];
15747 }
15748 } else {
15749 breaksCell[0].f1.size[0] = 1;
15750 breaksCell[0].f1.size[1] = 4;
15751 breaksCell[0].f1.data[0] = breakMat[0];
15752 breaksCell[0].f1.data[1] = breakMat[6];
15753 breaksCell[0].f1.data[2] = breakMat[12];
15754 breaksCell[0].f1.data[3] = breakMat[18];
15755 coeffCell[0].f1.size[0] = 18;
15756 coeffCell[0].f1.size[1] = 3;
15757 breaksCell[1].f1.size[0] = 1;
15758 breaksCell[1].f1.size[1] = 4;
15759 breaksCell[1].f1.data[0] = breakMat[0];
15760 breaksCell[1].f1.data[1] = breakMat[6];
15761 breaksCell[1].f1.data[2] = breakMat[12];
15762 breaksCell[1].f1.data[3] = breakMat[18];
15763 coeffCell[1].f1.size[0] = 18;
15764 coeffCell[1].f1.size[1] = 3;
15765 breaksCell[2].f1.size[0] = 1;
15766 breaksCell[2].f1.size[1] = 4;
15767 breaksCell[2].f1.data[0] = breakMat[0];
15768 breaksCell[2].f1.data[1] = breakMat[6];
15769 breaksCell[2].f1.data[2] = breakMat[12];
15770 breaksCell[2].f1.data[3] = breakMat[18];
15771 coeffCell[2].f1.size[0] = 18;
15772 coeffCell[2].f1.size[1] = 3;
15773 breaksCell[3].f1.size[0] = 1;
15774 breaksCell[3].f1.size[1] = 4;
15775 breaksCell[3].f1.data[0] = breakMat[0];
15776 breaksCell[3].f1.data[1] = breakMat[6];
15777 breaksCell[3].f1.data[2] = breakMat[12];
15778 breaksCell[3].f1.data[3] = breakMat[18];
15779 coeffCell[3].f1.size[0] = 18;
15780 coeffCell[3].f1.size[1] = 3;
15781 breaksCell[4].f1.size[0] = 1;
15782 breaksCell[4].f1.size[1] = 4;
15783 breaksCell[4].f1.data[0] = breakMat[0];
15784 breaksCell[4].f1.data[1] = breakMat[6];
15785 breaksCell[4].f1.data[2] = breakMat[12];
15786 breaksCell[4].f1.data[3] = breakMat[18];
15787 coeffCell[4].f1.size[0] = 18;
15788 coeffCell[4].f1.size[1] = 3;
15789 breaksCell[5].f1.size[0] = 1;
15790 breaksCell[5].f1.size[1] = 4;
15791 breaksCell[5].f1.data[0] = breakMat[0];
15792 breaksCell[5].f1.data[1] = breakMat[6];
15793 breaksCell[5].f1.data[2] = breakMat[12];
15794 breaksCell[5].f1.data[3] = breakMat[18];
15795 coeffCell[5].f1.size[0] = 18;
15796 coeffCell[5].f1.size[1] = 3;
15797 memcpy(&coeffCell[0].f1.data[0], &coeffMat[0], 54U * sizeof(real_T));
15798 memcpy(&coeffCell[1].f1.data[0], &coeffMat[0], 54U * sizeof(real_T));
15799 memcpy(&coeffCell[2].f1.data[0], &coeffMat[0], 54U * sizeof(real_T));
15800 memcpy(&coeffCell[3].f1.data[0], &coeffMat[0], 54U * sizeof(real_T));
15801 memcpy(&coeffCell[4].f1.data[0], &coeffMat[0], 54U * sizeof(real_T));
15802 memcpy(&coeffCell[5].f1.data[0], &coeffMat[0], 54U * sizeof(real_T));
15803 }
15804}
15805
15806static real_T cartesian_trajector_rt_powd_snf(real_T u0, real_T u1)
15807{
15808 real_T y;
15809 if (rtIsNaN(u0) || rtIsNaN(u1)) {
15810 y = (rtNaN);
15811 } else {
15812 cartesian_trajectory_planner__B.d1 = fabs(u0);
15813 cartesian_trajectory_planner__B.d2 = fabs(u1);
15814 if (rtIsInf(u1)) {
15815 if (cartesian_trajectory_planner__B.d1 == 1.0) {
15816 y = 1.0;
15817 } else if (cartesian_trajectory_planner__B.d1 > 1.0) {
15818 if (u1 > 0.0) {
15819 y = (rtInf);
15820 } else {
15821 y = 0.0;
15822 }
15823 } else if (u1 > 0.0) {
15824 y = 0.0;
15825 } else {
15826 y = (rtInf);
15827 }
15828 } else if (cartesian_trajectory_planner__B.d2 == 0.0) {
15829 y = 1.0;
15830 } else if (cartesian_trajectory_planner__B.d2 == 1.0) {
15831 if (u1 > 0.0) {
15832 y = u0;
15833 } else {
15834 y = 1.0 / u0;
15835 }
15836 } else if (u1 == 2.0) {
15837 y = u0 * u0;
15838 } else if ((u1 == 0.5) && (u0 >= 0.0)) {
15839 y = sqrt(u0);
15840 } else if ((u0 < 0.0) && (u1 > floor(u1))) {
15841 y = (rtNaN);
15842 } else {
15843 y = pow(u0, u1);
15844 }
15845 }
15846
15847 return y;
15848}
15849
15850static void ca_addFlatSegmentsToPPFormParts(const real_T oldbreaks_data[], const
15851 int32_T oldbreaks_size[2], const real_T oldCoeffs_data[], const int32_T
15852 oldCoeffs_size[2], real_T dim, real_T newBreaks_data[], int32_T
15853 newBreaks_size[2], real_T newCoefs_data[], int32_T newCoefs_size[2])
15854{
15855 cartesian_trajectory_planner__B.m_tmp = static_cast<int32_T>(dim);
15856 cartesian_trajectory_planner__B.m = cartesian_trajectory_planner__B.m_tmp - 1;
15857 for (cartesian_trajectory_planner__B.b_i = 0;
15858 cartesian_trajectory_planner__B.b_i <= cartesian_trajectory_planner__B.m;
15859 cartesian_trajectory_planner__B.b_i++) {
15860 cartesian_trajectory_planner__B.d_n = cartesian_trajectory_planner__B.m_tmp
15861 + cartesian_trajectory_planner__B.b_i;
15862 cartesian_trajectory_planner__B.s =
15863 oldCoeffs_data[cartesian_trajectory_planner__B.b_i %
15864 cartesian_trajectory_planner__B.m_tmp + oldCoeffs_size[0] *
15865 div_nzp_s32_floor(cartesian_trajectory_planner__B.b_i,
15866 cartesian_trajectory_planner__B.m_tmp)] * 0.0 +
15867 oldCoeffs_data[cartesian_trajectory_planner__B.d_n %
15868 cartesian_trajectory_planner__B.m_tmp + oldCoeffs_size[0] *
15869 div_nzp_s32_floor(cartesian_trajectory_planner__B.d_n,
15870 cartesian_trajectory_planner__B.m_tmp)] * 0.0;
15871 cartesian_trajectory_planner__B.d_n = (cartesian_trajectory_planner__B.m_tmp
15872 << 1) + cartesian_trajectory_planner__B.b_i;
15873 cartesian_trajectory_planner__B.valueAtStart_data[cartesian_trajectory_planner__B.b_i]
15874 = oldCoeffs_data[cartesian_trajectory_planner__B.d_n %
15875 cartesian_trajectory_planner__B.m_tmp + oldCoeffs_size[0] *
15876 div_nzp_s32_floor(cartesian_trajectory_planner__B.d_n,
15877 cartesian_trajectory_planner__B.m_tmp)] +
15878 cartesian_trajectory_planner__B.s;
15879 }
15880
15881 cartesian_trajectory_planner__B.loop_ub_tmp =
15882 cartesian_trajectory_planner__B.m_tmp * 3 - 1;
15883 if (0 <= cartesian_trajectory_planner__B.loop_ub_tmp) {
15884 memset(&cartesian_trajectory_planner__B.newSegmentCoeffs_data[0], 0,
15885 (cartesian_trajectory_planner__B.loop_ub_tmp + 1) * sizeof(real_T));
15886 }
15887
15888 for (cartesian_trajectory_planner__B.d_n = 0;
15889 cartesian_trajectory_planner__B.d_n <
15890 cartesian_trajectory_planner__B.m_tmp;
15891 cartesian_trajectory_planner__B.d_n++) {
15892 cartesian_trajectory_planner__B.newSegmentCoeffs_data[cartesian_trajectory_planner__B.d_n
15893 + (cartesian_trajectory_planner__B.m_tmp << 1)] =
15894 cartesian_trajectory_planner__B.valueAtStart_data[cartesian_trajectory_planner__B.d_n];
15895 }
15896
15897 cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_t = static_cast<
15898 real_T>(oldCoeffs_size[0]) + dim;
15899 cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_c = static_cast<
15900 int32_T>(cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_t);
15901 cartesian_trajectory_planner__B.loop_ub_m =
15902 cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_c * 3 - 1;
15903 if (0 <= cartesian_trajectory_planner__B.loop_ub_m) {
15904 memset(&cartesian_trajectory_planner__B.coefsWithFlatStart_data[0], 0,
15905 (cartesian_trajectory_planner__B.loop_ub_m + 1) * sizeof(real_T));
15906 }
15907
15908 for (cartesian_trajectory_planner__B.d_n = 0;
15909 cartesian_trajectory_planner__B.d_n < 3;
15910 cartesian_trajectory_planner__B.d_n++) {
15911 for (cartesian_trajectory_planner__B.b_i = 0;
15912 cartesian_trajectory_planner__B.b_i <
15913 cartesian_trajectory_planner__B.m_tmp;
15914 cartesian_trajectory_planner__B.b_i++) {
15915 cartesian_trajectory_planner__B.coefsWithFlatStart_data[cartesian_trajectory_planner__B.b_i
15916 + cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_c *
15917 cartesian_trajectory_planner__B.d_n] =
15918 cartesian_trajectory_planner__B.newSegmentCoeffs_data[cartesian_trajectory_planner__B.m_tmp
15919 * cartesian_trajectory_planner__B.d_n +
15920 cartesian_trajectory_planner__B.b_i];
15921 }
15922 }
15923
15924 if (dim + 1.0 >
15925 cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_t) {
15926 cartesian_trajectory_planner__B.h = 0;
15927 } else {
15928 cartesian_trajectory_planner__B.h = static_cast<int32_T>(dim + 1.0) - 1;
15929 }
15930
15931 cartesian_trajectory_planner__B.loop_ub_m = oldCoeffs_size[0];
15932 for (cartesian_trajectory_planner__B.d_n = 0;
15933 cartesian_trajectory_planner__B.d_n < 3;
15934 cartesian_trajectory_planner__B.d_n++) {
15935 for (cartesian_trajectory_planner__B.b_i = 0;
15936 cartesian_trajectory_planner__B.b_i <
15937 cartesian_trajectory_planner__B.loop_ub_m;
15938 cartesian_trajectory_planner__B.b_i++) {
15939 cartesian_trajectory_planner__B.coefsWithFlatStart_data
15940 [(cartesian_trajectory_planner__B.h +
15941 cartesian_trajectory_planner__B.b_i) +
15942 cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_c *
15943 cartesian_trajectory_planner__B.d_n] = oldCoeffs_data[oldCoeffs_size[0] *
15944 cartesian_trajectory_planner__B.d_n +
15945 cartesian_trajectory_planner__B.b_i];
15946 }
15947 }
15948
15949 cartesian_trajectory_planner__B.h = oldbreaks_size[1] + 1;
15950 cartesian_trajectory_planner__B.loop_ub_m = oldbreaks_size[0] *
15951 oldbreaks_size[1] - 1;
15952 cartesian_trajectory_planner__B.valueAtStart_data[0] = oldbreaks_data[0] - 1.0;
15953 if (0 <= cartesian_trajectory_planner__B.loop_ub_m) {
15954 memcpy(&cartesian_trajectory_planner__B.valueAtStart_data[1],
15955 &oldbreaks_data[0], (cartesian_trajectory_planner__B.loop_ub_m + 1) *
15956 sizeof(real_T));
15957 }
15958
15959 cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_t =
15960 cartesian_trajectory_planner__B.valueAtStart_data[cartesian_trajectory_planner__B.h
15961 - 1];
15962 cartesian_trajectory_planner__B.holdPoint =
15963 cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_t -
15964 cartesian_trajectory_planner__B.valueAtStart_data[cartesian_trajectory_planner__B.h
15965 - 2];
15966 cartesian_trajectory_planner__B.B_idx_0 = cartesian_trajector_rt_powd_snf
15967 (cartesian_trajectory_planner__B.holdPoint, 2.0);
15968 cartesian_trajectory_planner__B.B_idx_1 = cartesian_trajector_rt_powd_snf
15969 (cartesian_trajectory_planner__B.holdPoint, 1.0);
15970 cartesian_trajectory_planner__B.holdPoint = cartesian_trajector_rt_powd_snf
15971 (cartesian_trajectory_planner__B.holdPoint, 0.0);
15972 cartesian_trajectory_planner__B.s = (static_cast<real_T>
15973 (cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_c) - dim) +
15974 1.0;
15975 if (cartesian_trajectory_planner__B.s >
15976 cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_c) {
15977 cartesian_trajectory_planner__B.loop_ub_m = 0;
15978 cartesian_trajectory_planner__B.d_n = 0;
15979 } else {
15980 cartesian_trajectory_planner__B.loop_ub_m = static_cast<int32_T>
15981 (cartesian_trajectory_planner__B.s) - 1;
15982 cartesian_trajectory_planner__B.d_n =
15983 cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_c;
15984 }
15985
15986 cartesian_trajectory_planner__B.m_tmp_m = cartesian_trajectory_planner__B.d_n
15987 - cartesian_trajectory_planner__B.loop_ub_m;
15988 cartesian_trajectory_planner__B.m = cartesian_trajectory_planner__B.m_tmp_m -
15989 1;
15990 for (cartesian_trajectory_planner__B.b_i = 0;
15991 cartesian_trajectory_planner__B.b_i <= cartesian_trajectory_planner__B.m;
15992 cartesian_trajectory_planner__B.b_i++) {
15993 cartesian_trajectory_planner__B.d_n =
15994 cartesian_trajectory_planner__B.m_tmp_m +
15995 cartesian_trajectory_planner__B.b_i;
15996 cartesian_trajectory_planner__B.s =
15997 cartesian_trajectory_planner__B.coefsWithFlatStart_data
15998 [(cartesian_trajectory_planner__B.b_i %
15999 cartesian_trajectory_planner__B.m_tmp_m +
16000 cartesian_trajectory_planner__B.loop_ub_m) + div_nzp_s32_floor
16001 (cartesian_trajectory_planner__B.b_i,
16002 cartesian_trajectory_planner__B.m_tmp_m) *
16003 cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_c] *
16004 cartesian_trajectory_planner__B.B_idx_0 +
16005 cartesian_trajectory_planner__B.coefsWithFlatStart_data
16006 [(cartesian_trajectory_planner__B.d_n %
16007 cartesian_trajectory_planner__B.m_tmp_m +
16008 cartesian_trajectory_planner__B.loop_ub_m) + div_nzp_s32_floor
16009 (cartesian_trajectory_planner__B.d_n,
16010 cartesian_trajectory_planner__B.m_tmp_m) *
16011 cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_c] *
16012 cartesian_trajectory_planner__B.B_idx_1;
16013 cartesian_trajectory_planner__B.d_n =
16014 (cartesian_trajectory_planner__B.m_tmp_m << 1) +
16015 cartesian_trajectory_planner__B.b_i;
16016 cartesian_trajectory_planner__B.valueAtEnd_data[cartesian_trajectory_planner__B.b_i]
16017 = cartesian_trajectory_planner__B.coefsWithFlatStart_data
16018 [(cartesian_trajectory_planner__B.d_n %
16019 cartesian_trajectory_planner__B.m_tmp_m +
16020 cartesian_trajectory_planner__B.loop_ub_m) + div_nzp_s32_floor
16021 (cartesian_trajectory_planner__B.d_n,
16022 cartesian_trajectory_planner__B.m_tmp_m) *
16023 cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_c] *
16024 cartesian_trajectory_planner__B.holdPoint
16025 + cartesian_trajectory_planner__B.s;
16026 }
16027
16028 if (0 <= cartesian_trajectory_planner__B.loop_ub_tmp) {
16029 memset(&cartesian_trajectory_planner__B.newSegmentCoeffs_data[0], 0,
16030 (cartesian_trajectory_planner__B.loop_ub_tmp + 1) * sizeof(real_T));
16031 }
16032
16033 for (cartesian_trajectory_planner__B.d_n = 0;
16034 cartesian_trajectory_planner__B.d_n <
16035 cartesian_trajectory_planner__B.m_tmp_m;
16036 cartesian_trajectory_planner__B.d_n++) {
16037 cartesian_trajectory_planner__B.newSegmentCoeffs_data[cartesian_trajectory_planner__B.d_n
16038 + (cartesian_trajectory_planner__B.m_tmp << 1)] =
16039 cartesian_trajectory_planner__B.valueAtEnd_data[cartesian_trajectory_planner__B.d_n];
16040 }
16041
16042 cartesian_trajectory_planner__B.loop_ub_tmp = static_cast<int32_T>(
16043 static_cast<real_T>
16044 (cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_c) + dim);
16045 newCoefs_size[0] = cartesian_trajectory_planner__B.loop_ub_tmp;
16046 newCoefs_size[1] = 3;
16047 cartesian_trajectory_planner__B.loop_ub_m =
16048 cartesian_trajectory_planner__B.loop_ub_tmp * 3 - 1;
16049 if (0 <= cartesian_trajectory_planner__B.loop_ub_m) {
16050 memset(&newCoefs_data[0], 0, (cartesian_trajectory_planner__B.loop_ub_m + 1)
16051 * sizeof(real_T));
16052 }
16053
16054 for (cartesian_trajectory_planner__B.d_n = 0;
16055 cartesian_trajectory_planner__B.d_n < 3;
16056 cartesian_trajectory_planner__B.d_n++) {
16057 for (cartesian_trajectory_planner__B.b_i = 0;
16058 cartesian_trajectory_planner__B.b_i <
16059 cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_c;
16060 cartesian_trajectory_planner__B.b_i++) {
16061 newCoefs_data[cartesian_trajectory_planner__B.b_i +
16062 cartesian_trajectory_planner__B.loop_ub_tmp *
16063 cartesian_trajectory_planner__B.d_n] =
16064 cartesian_trajectory_planner__B.coefsWithFlatStart_data[cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_c
16065 * cartesian_trajectory_planner__B.d_n +
16066 cartesian_trajectory_planner__B.b_i];
16067 }
16068 }
16069
16070 for (cartesian_trajectory_planner__B.d_n = 0;
16071 cartesian_trajectory_planner__B.d_n < 3;
16072 cartesian_trajectory_planner__B.d_n++) {
16073 for (cartesian_trajectory_planner__B.b_i = 0;
16074 cartesian_trajectory_planner__B.b_i <
16075 cartesian_trajectory_planner__B.m_tmp;
16076 cartesian_trajectory_planner__B.b_i++) {
16077 newCoefs_data
16078 [((cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_c +
16079 (cartesian_trajectory_planner__B.b_i + 1)) +
16080 cartesian_trajectory_planner__B.loop_ub_tmp *
16081 cartesian_trajectory_planner__B.d_n) - 1] =
16082 cartesian_trajectory_planner__B.newSegmentCoeffs_data[cartesian_trajectory_planner__B.m_tmp
16083 * cartesian_trajectory_planner__B.d_n +
16084 cartesian_trajectory_planner__B.b_i];
16085 }
16086 }
16087
16088 newBreaks_size[0] = 1;
16089 newBreaks_size[1] = cartesian_trajectory_planner__B.h + 1;
16090 if (0 <= cartesian_trajectory_planner__B.h - 1) {
16091 memcpy(&newBreaks_data[0],
16092 &cartesian_trajectory_planner__B.valueAtStart_data[0],
16093 cartesian_trajectory_planner__B.h * sizeof(real_T));
16094 }
16095
16096 newBreaks_data[cartesian_trajectory_planner__B.h] =
16097 cartesian_trajectory_planner__B.coefsWithFlatStart_size_idx_0_t + 1.0;
16098}
16099
16100static void cartesian__polyCoeffsDerivative(const real_T coeffs_data[], const
16101 int32_T coeffs_size[2], real_T dCoeffs_data[], int32_T dCoeffs_size[2])
16102{
16103 cartesian_trajectory_planner__B.b_idx_0 = coeffs_size[0];
16104 dCoeffs_size[0] = coeffs_size[0];
16105 dCoeffs_size[1] = 3;
16106 cartesian_trajectory_planner__B.loop_ub_o4 = coeffs_size[0] * 3 - 1;
16107 if (0 <= cartesian_trajectory_planner__B.loop_ub_o4) {
16108 memset(&dCoeffs_data[0], 0, (cartesian_trajectory_planner__B.loop_ub_o4 + 1)
16109 * sizeof(real_T));
16110 }
16111
16112 cartesian_trajectory_planner__B.loop_ub_o4 = coeffs_size[0];
16113 for (cartesian_trajectory_planner__B.i2 = 0;
16114 cartesian_trajectory_planner__B.i2 <
16115 cartesian_trajectory_planner__B.loop_ub_o4;
16116 cartesian_trajectory_planner__B.i2++) {
16117 dCoeffs_data[cartesian_trajectory_planner__B.i2 +
16118 cartesian_trajectory_planner__B.b_idx_0] = 2.0 *
16119 coeffs_data[cartesian_trajectory_planner__B.i2];
16120 }
16121
16122 cartesian_trajectory_planner__B.loop_ub_o4 = coeffs_size[0];
16123 for (cartesian_trajectory_planner__B.i2 = 0;
16124 cartesian_trajectory_planner__B.i2 <
16125 cartesian_trajectory_planner__B.loop_ub_o4;
16126 cartesian_trajectory_planner__B.i2++) {
16127 dCoeffs_data[cartesian_trajectory_planner__B.i2 +
16128 (cartesian_trajectory_planner__B.b_idx_0 << 1)] =
16129 coeffs_data[cartesian_trajectory_planner__B.i2 + coeffs_size[0]];
16130 }
16131}
16132
16133static int32_T cartesian_trajectory_pl_bsearch(const real_T x_data[], const
16134 int32_T x_size[2], real_T xi)
16135{
16136 int32_T n;
16137 n = 1;
16138 cartesian_trajectory_planner__B.low_ip1 = 1;
16139 cartesian_trajectory_planner__B.high_i = x_size[1];
16140 while (cartesian_trajectory_planner__B.high_i >
16141 cartesian_trajectory_planner__B.low_ip1 + 1) {
16142 cartesian_trajectory_planner__B.mid_i = (n >> 1) +
16143 (cartesian_trajectory_planner__B.high_i >> 1);
16144 if (((n & 1) == 1) && ((cartesian_trajectory_planner__B.high_i & 1) == 1)) {
16145 cartesian_trajectory_planner__B.mid_i++;
16146 }
16147
16148 if (xi >= x_data[cartesian_trajectory_planner__B.mid_i - 1]) {
16149 n = cartesian_trajectory_planner__B.mid_i;
16150 cartesian_trajectory_planner__B.low_ip1 =
16151 cartesian_trajectory_planner__B.mid_i;
16152 } else {
16153 cartesian_trajectory_planner__B.high_i =
16154 cartesian_trajectory_planner__B.mid_i;
16155 }
16156 }
16157
16158 return n;
16159}
16160
16161static void cartesian_trajectory_plan_ppval(const real_T pp_breaks_data[], const
16162 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], const int32_T
16163 pp_coefs_size[3], const real_T x[2], real_T v_data[], int32_T v_size[2])
16164{
16165 cartesian_trajectory_planner__B.elementsPerPage = pp_coefs_size[0] - 1;
16166 cartesian_trajectory_planner__B.coefStride = (pp_breaks_size[1] - 1) *
16167 pp_coefs_size[0];
16168 v_size[0] = pp_coefs_size[0];
16169 v_size[1] = 2;
16170 if (pp_coefs_size[0] == 1) {
16171 if (rtIsNaN(x[0])) {
16172 v_data[0] = x[0];
16173 } else {
16174 cartesian_trajectory_planner__B.ip_p = cartesian_trajectory_pl_bsearch
16175 (pp_breaks_data, pp_breaks_size, x[0]) - 1;
16176 cartesian_trajectory_planner__B.xloc_b = x[0] -
16177 pp_breaks_data[cartesian_trajectory_planner__B.ip_p];
16178 v_data[0] = (cartesian_trajectory_planner__B.xloc_b *
16179 pp_coefs_data[cartesian_trajectory_planner__B.ip_p] +
16180 pp_coefs_data[cartesian_trajectory_planner__B.ip_p +
16181 cartesian_trajectory_planner__B.coefStride]) *
16182 cartesian_trajectory_planner__B.xloc_b + pp_coefs_data
16183 [(cartesian_trajectory_planner__B.coefStride << 1) +
16184 cartesian_trajectory_planner__B.ip_p];
16185 }
16186
16187 if (rtIsNaN(x[1])) {
16188 v_data[1] = x[1];
16189 } else {
16190 cartesian_trajectory_planner__B.ip_p = cartesian_trajectory_pl_bsearch
16191 (pp_breaks_data, pp_breaks_size, x[1]) - 1;
16192 cartesian_trajectory_planner__B.xloc_b = x[1] -
16193 pp_breaks_data[cartesian_trajectory_planner__B.ip_p];
16194 v_data[1] = (cartesian_trajectory_planner__B.xloc_b *
16195 pp_coefs_data[cartesian_trajectory_planner__B.ip_p] +
16196 pp_coefs_data[cartesian_trajectory_planner__B.ip_p +
16197 cartesian_trajectory_planner__B.coefStride]) *
16198 cartesian_trajectory_planner__B.xloc_b + pp_coefs_data
16199 [(cartesian_trajectory_planner__B.coefStride << 1) +
16200 cartesian_trajectory_planner__B.ip_p];
16201 }
16202 } else {
16203 if (rtIsNaN(x[0])) {
16204 for (cartesian_trajectory_planner__B.ip_p = 0;
16205 cartesian_trajectory_planner__B.ip_p <=
16206 cartesian_trajectory_planner__B.elementsPerPage;
16207 cartesian_trajectory_planner__B.ip_p++) {
16208 v_data[cartesian_trajectory_planner__B.ip_p] = x[0];
16209 }
16210 } else {
16211 cartesian_trajectory_planner__B.ip_p = cartesian_trajectory_pl_bsearch
16212 (pp_breaks_data, pp_breaks_size, x[0]) - 1;
16213 cartesian_trajectory_planner__B.icp =
16214 (cartesian_trajectory_planner__B.elementsPerPage + 1) *
16215 cartesian_trajectory_planner__B.ip_p;
16216 cartesian_trajectory_planner__B.xloc_b = x[0] -
16217 pp_breaks_data[cartesian_trajectory_planner__B.ip_p];
16218 cartesian_trajectory_planner__B.ic0 = (cartesian_trajectory_planner__B.icp
16219 + cartesian_trajectory_planner__B.coefStride) - 1;
16220 for (cartesian_trajectory_planner__B.ip_p = 0;
16221 cartesian_trajectory_planner__B.ip_p <=
16222 cartesian_trajectory_planner__B.elementsPerPage;
16223 cartesian_trajectory_planner__B.ip_p++) {
16224 v_data[cartesian_trajectory_planner__B.ip_p] =
16225 pp_coefs_data[cartesian_trajectory_planner__B.icp +
16226 cartesian_trajectory_planner__B.ip_p];
16227 cartesian_trajectory_planner__B.j = cartesian_trajectory_planner__B.ip_p
16228 + 1;
16229 v_data[-1 + cartesian_trajectory_planner__B.j] = v_data[-1 +
16230 cartesian_trajectory_planner__B.j] *
16231 cartesian_trajectory_planner__B.xloc_b +
16232 pp_coefs_data[cartesian_trajectory_planner__B.ic0 +
16233 cartesian_trajectory_planner__B.j];
16234 }
16235
16236 cartesian_trajectory_planner__B.ic0 =
16237 ((cartesian_trajectory_planner__B.coefStride << 1) +
16238 cartesian_trajectory_planner__B.icp) - 1;
16239 for (cartesian_trajectory_planner__B.ip_p = 0;
16240 cartesian_trajectory_planner__B.ip_p <=
16241 cartesian_trajectory_planner__B.elementsPerPage;
16242 cartesian_trajectory_planner__B.ip_p++) {
16243 cartesian_trajectory_planner__B.j = cartesian_trajectory_planner__B.ip_p
16244 + 1;
16245 v_data[-1 + cartesian_trajectory_planner__B.j] = v_data[-1 +
16246 cartesian_trajectory_planner__B.j] *
16247 cartesian_trajectory_planner__B.xloc_b +
16248 pp_coefs_data[cartesian_trajectory_planner__B.ic0 +
16249 cartesian_trajectory_planner__B.j];
16250 }
16251 }
16252
16253 if (rtIsNaN(x[1])) {
16254 for (cartesian_trajectory_planner__B.ip_p = 0;
16255 cartesian_trajectory_planner__B.ip_p <=
16256 cartesian_trajectory_planner__B.elementsPerPage;
16257 cartesian_trajectory_planner__B.ip_p++) {
16258 v_data[(cartesian_trajectory_planner__B.elementsPerPage +
16259 cartesian_trajectory_planner__B.ip_p) + 1] = x[1];
16260 }
16261 } else {
16262 cartesian_trajectory_planner__B.ip_p = cartesian_trajectory_pl_bsearch
16263 (pp_breaks_data, pp_breaks_size, x[1]) - 1;
16264 cartesian_trajectory_planner__B.icp =
16265 (cartesian_trajectory_planner__B.elementsPerPage + 1) *
16266 cartesian_trajectory_planner__B.ip_p;
16267 cartesian_trajectory_planner__B.xloc_b = x[1] -
16268 pp_breaks_data[cartesian_trajectory_planner__B.ip_p];
16269 cartesian_trajectory_planner__B.ic0 = (cartesian_trajectory_planner__B.icp
16270 + cartesian_trajectory_planner__B.coefStride) - 1;
16271 for (cartesian_trajectory_planner__B.ip_p = 0;
16272 cartesian_trajectory_planner__B.ip_p <=
16273 cartesian_trajectory_planner__B.elementsPerPage;
16274 cartesian_trajectory_planner__B.ip_p++) {
16275 v_data[(cartesian_trajectory_planner__B.elementsPerPage +
16276 cartesian_trajectory_planner__B.ip_p) + 1] =
16277 pp_coefs_data[cartesian_trajectory_planner__B.icp +
16278 cartesian_trajectory_planner__B.ip_p];
16279 cartesian_trajectory_planner__B.j = cartesian_trajectory_planner__B.ip_p
16280 + 1;
16281 cartesian_trajectory_planner__B.v_data_tmp =
16282 cartesian_trajectory_planner__B.elementsPerPage +
16283 cartesian_trajectory_planner__B.j;
16284 v_data[cartesian_trajectory_planner__B.v_data_tmp] =
16285 v_data[cartesian_trajectory_planner__B.v_data_tmp] *
16286 cartesian_trajectory_planner__B.xloc_b +
16287 pp_coefs_data[cartesian_trajectory_planner__B.ic0 +
16288 cartesian_trajectory_planner__B.j];
16289 }
16290
16291 cartesian_trajectory_planner__B.ic0 =
16292 ((cartesian_trajectory_planner__B.coefStride << 1) +
16293 cartesian_trajectory_planner__B.icp) - 1;
16294 for (cartesian_trajectory_planner__B.ip_p = 0;
16295 cartesian_trajectory_planner__B.ip_p <=
16296 cartesian_trajectory_planner__B.elementsPerPage;
16297 cartesian_trajectory_planner__B.ip_p++) {
16298 cartesian_trajectory_planner__B.j = cartesian_trajectory_planner__B.ip_p
16299 + 1;
16300 cartesian_trajectory_planner__B.v_data_tmp =
16301 cartesian_trajectory_planner__B.elementsPerPage +
16302 cartesian_trajectory_planner__B.j;
16303 v_data[cartesian_trajectory_planner__B.v_data_tmp] =
16304 v_data[cartesian_trajectory_planner__B.v_data_tmp] *
16305 cartesian_trajectory_planner__B.xloc_b +
16306 pp_coefs_data[cartesian_trajectory_planner__B.ic0 +
16307 cartesian_trajectory_planner__B.j];
16308 }
16309 }
16310 }
16311}
16312
16313static void c_generateTrajectoriesFromCoefs(const real_T breaks_data[], const
16314 int32_T breaks_size[2], const real_T coeffs_data[], const int32_T coeffs_size
16315 [2], real_T dim, const real_T t[2], real_T q_data[], int32_T q_size[2], real_T
16316 qd_data[], int32_T qd_size[2], real_T qdd_data[], int32_T qdd_size[2], real_T
16317 pp_breaks_data[], int32_T pp_breaks_size[2], real_T pp_coefs_data[], int32_T
16318 pp_coefs_size[3])
16319{
16320 ca_addFlatSegmentsToPPFormParts(breaks_data, breaks_size, coeffs_data,
16321 coeffs_size, dim, cartesian_trajectory_planner__B.modBreaks_data,
16322 cartesian_trajectory_planner__B.modBreaks_size,
16323 cartesian_trajectory_planner__B.modCoeffs_data,
16324 cartesian_trajectory_planner__B.modCoeffs_size);
16325 cartesian__polyCoeffsDerivative(cartesian_trajectory_planner__B.modCoeffs_data,
16326 cartesian_trajectory_planner__B.modCoeffs_size,
16327 cartesian_trajectory_planner__B.tmp_data,
16328 cartesian_trajectory_planner__B.tmp_size);
16329 cartesian_trajectory_planner__B.dCoeffs_size[0] =
16330 cartesian_trajectory_planner__B.tmp_size[0];
16331 cartesian_trajectory_planner__B.dCoeffs_size[1] = 3;
16332 cartesian_trajectory_planner__B.loop_ub_o =
16333 cartesian_trajectory_planner__B.tmp_size[0] *
16334 cartesian_trajectory_planner__B.tmp_size[1];
16335 if (0 <= cartesian_trajectory_planner__B.loop_ub_o - 1) {
16336 memcpy(&cartesian_trajectory_planner__B.dCoeffs_data[0],
16337 &cartesian_trajectory_planner__B.tmp_data[0],
16338 cartesian_trajectory_planner__B.loop_ub_o * sizeof(real_T));
16339 }
16340
16341 cartesian_trajectory_planner__B.num_idx_0_tmp = static_cast<int32_T>(dim);
16342 cartesian_trajectory_planner__B.num_idx_1 =
16343 cartesian_trajectory_planner__B.modBreaks_size[1] - 1;
16344 pp_breaks_size[0] = 1;
16345 pp_breaks_size[1] = cartesian_trajectory_planner__B.modBreaks_size[1];
16346 cartesian_trajectory_planner__B.loop_ub_o =
16347 cartesian_trajectory_planner__B.modBreaks_size[1];
16348 if (0 <= cartesian_trajectory_planner__B.loop_ub_o - 1) {
16349 memcpy(&pp_breaks_data[0], &cartesian_trajectory_planner__B.modBreaks_data[0],
16350 cartesian_trajectory_planner__B.loop_ub_o * sizeof(real_T));
16351 }
16352
16353 pp_coefs_size[0] = cartesian_trajectory_planner__B.num_idx_0_tmp;
16354 pp_coefs_size[1] = cartesian_trajectory_planner__B.num_idx_1;
16355 pp_coefs_size[2] = 3;
16356 cartesian_trajectory_planner__B.loop_ub_o =
16357 cartesian_trajectory_planner__B.num_idx_0_tmp *
16358 cartesian_trajectory_planner__B.num_idx_1 * 3 - 1;
16359 if (0 <= cartesian_trajectory_planner__B.loop_ub_o) {
16360 memcpy(&pp_coefs_data[0], &cartesian_trajectory_planner__B.modCoeffs_data[0],
16361 (cartesian_trajectory_planner__B.loop_ub_o + 1) * sizeof(real_T));
16362 }
16363
16364 cartesian_trajectory_planner__B.num[0] =
16365 cartesian_trajectory_planner__B.num_idx_0_tmp;
16366 cartesian_trajectory_planner__B.num[1] =
16367 cartesian_trajectory_planner__B.num_idx_1;
16368 cartesian_trajectory_planner__B.num[2] = 3;
16369 cartesian_trajectory_planner__B.modBreaks_size_n[0] = 1;
16370 cartesian_trajectory_planner__B.modBreaks_size_n[1] =
16371 cartesian_trajectory_planner__B.modBreaks_size[1];
16372 cartesian_trajectory_planner__B.loop_ub_o =
16373 cartesian_trajectory_planner__B.modBreaks_size[1];
16374 if (0 <= cartesian_trajectory_planner__B.loop_ub_o - 1) {
16375 memcpy(&cartesian_trajectory_planner__B.modBreaks_data_f[0],
16376 &cartesian_trajectory_planner__B.modBreaks_data[0],
16377 cartesian_trajectory_planner__B.loop_ub_o * sizeof(real_T));
16378 }
16379
16380 cartesian_trajectory_plan_ppval
16381 (cartesian_trajectory_planner__B.modBreaks_data_f,
16382 cartesian_trajectory_planner__B.modBreaks_size_n,
16383 cartesian_trajectory_planner__B.modCoeffs_data,
16384 cartesian_trajectory_planner__B.num, t, q_data, q_size);
16385 cartesian_trajectory_planner__B.num[0] =
16386 cartesian_trajectory_planner__B.num_idx_0_tmp;
16387 cartesian_trajectory_planner__B.num[1] =
16388 cartesian_trajectory_planner__B.modBreaks_size[1] - 1;
16389 cartesian_trajectory_planner__B.num[2] = 3;
16390 cartesian_trajectory_planner__B.modBreaks_size_b[0] = 1;
16391 cartesian_trajectory_planner__B.modBreaks_size_b[1] =
16392 cartesian_trajectory_planner__B.modBreaks_size[1];
16393 cartesian_trajectory_planner__B.loop_ub_o =
16394 cartesian_trajectory_planner__B.modBreaks_size[1];
16395 if (0 <= cartesian_trajectory_planner__B.loop_ub_o - 1) {
16396 memcpy(&cartesian_trajectory_planner__B.modBreaks_data_f[0],
16397 &cartesian_trajectory_planner__B.modBreaks_data[0],
16398 cartesian_trajectory_planner__B.loop_ub_o * sizeof(real_T));
16399 }
16400
16401 cartesian_trajectory_plan_ppval
16402 (cartesian_trajectory_planner__B.modBreaks_data_f,
16403 cartesian_trajectory_planner__B.modBreaks_size_b,
16404 cartesian_trajectory_planner__B.dCoeffs_data,
16405 cartesian_trajectory_planner__B.num, t, qd_data, qd_size);
16406 cartesian__polyCoeffsDerivative(cartesian_trajectory_planner__B.dCoeffs_data,
16407 cartesian_trajectory_planner__B.dCoeffs_size,
16408 cartesian_trajectory_planner__B.tmp_data,
16409 cartesian_trajectory_planner__B.tmp_size);
16410 cartesian_trajectory_planner__B.num[0] =
16411 cartesian_trajectory_planner__B.num_idx_0_tmp;
16412 cartesian_trajectory_planner__B.num[1] =
16413 cartesian_trajectory_planner__B.modBreaks_size[1] - 1;
16414 cartesian_trajectory_planner__B.num[2] = 3;
16415 cartesian_trajectory_planner__B.modBreaks_size_l[0] = 1;
16416 cartesian_trajectory_planner__B.modBreaks_size_l[1] =
16417 cartesian_trajectory_planner__B.modBreaks_size[1];
16418 cartesian_trajectory_planner__B.loop_ub_o =
16419 cartesian_trajectory_planner__B.modBreaks_size[1];
16420 if (0 <= cartesian_trajectory_planner__B.loop_ub_o - 1) {
16421 memcpy(&cartesian_trajectory_planner__B.modBreaks_data_f[0],
16422 &cartesian_trajectory_planner__B.modBreaks_data[0],
16423 cartesian_trajectory_planner__B.loop_ub_o * sizeof(real_T));
16424 }
16425
16426 cartesian_trajectory_plan_ppval
16427 (cartesian_trajectory_planner__B.modBreaks_data_f,
16428 cartesian_trajectory_planner__B.modBreaks_size_l,
16429 cartesian_trajectory_planner__B.tmp_data,
16430 cartesian_trajectory_planner__B.num, t, qdd_data, qdd_size);
16431}
16432
16433static void cartesian_trajector_trapveltraj(const real_T wayPoints[12], const
16434 real_T varargin_2[6], real_T varargin_4, real_T q[12], real_T qd[12], real_T
16435 qdd[12], real_T t[2], s_06c2DDfmr4zcnTqhww20ZC_cart_T ppCell_data[], int32_T
16436 *ppCell_size)
16437{
16438 boolean_T exitg1;
16439 NameValueParser_NameValueParser(&cartesian_trajectory_planner__B.parser);
16440 for (cartesian_trajectory_planner__B.i_i = 0;
16441 cartesian_trajectory_planner__B.i_i < 6;
16442 cartesian_trajectory_planner__B.i_i++) {
16443 cartesian_trajectory_planner__B.parser.ParsedResults.f1[cartesian_trajectory_planner__B.i_i]
16444 = varargin_2[cartesian_trajectory_planner__B.i_i];
16445 }
16446
16447 cartesian_trajectory_planner__B.parser.ParsedResults.f2 = varargin_4;
16448 for (cartesian_trajectory_planner__B.i_i = 0;
16449 cartesian_trajectory_planner__B.i_i < 6;
16450 cartesian_trajectory_planner__B.i_i++) {
16451 cartesian_trajectory_planner__B.a[cartesian_trajectory_planner__B.i_i] =
16452 cartesian_trajectory_planner__B.parser.ParsedResults.f1[cartesian_trajectory_planner__B.i_i];
16453 }
16454
16455 cartesian_trajectory_planner__B.acc =
16456 cartesian_trajectory_planner__B.parser.ParsedResults.f2;
16457 for (cartesian_trajectory_planner__B.i_i = 0;
16458 cartesian_trajectory_planner__B.i_i < 6;
16459 cartesian_trajectory_planner__B.i_i++) {
16460 cartesian_trajectory_planner__B.vel[cartesian_trajectory_planner__B.i_i] =
16461 cartesian_trajectory_planner__B.a[cartesian_trajectory_planner__B.i_i];
16462 cartesian_trajectory_planner__B.matrixInput_data[cartesian_trajectory_planner__B.i_i]
16463 = cartesian_trajectory_planner__B.acc;
16464 }
16465
16466 for (cartesian_trajectory_planner__B.i_i = 0;
16467 cartesian_trajectory_planner__B.i_i < 12;
16468 cartesian_trajectory_planner__B.i_i++) {
16469 q[cartesian_trajectory_planner__B.i_i] = 0.0;
16470 }
16471
16472 for (cartesian_trajectory_planner__B.i_i = 0;
16473 cartesian_trajectory_planner__B.i_i < 12;
16474 cartesian_trajectory_planner__B.i_i++) {
16475 qd[cartesian_trajectory_planner__B.i_i] = 0.0;
16476 }
16477
16478 for (cartesian_trajectory_planner__B.i_i = 0;
16479 cartesian_trajectory_planner__B.i_i < 12;
16480 cartesian_trajectory_planner__B.i_i++) {
16481 qdd[cartesian_trajectory_planner__B.i_i] = 0.0;
16482 }
16483
16484 memset(&cartesian_trajectory_planner__B.coeffMat[0], 0, 54U * sizeof(real_T));
16485 memset(&cartesian_trajectory_planner__B.breakMat[0], 0, 24U * sizeof(real_T));
16486 for (cartesian_trajectory_planner__B.b_idx = 0;
16487 cartesian_trajectory_planner__B.b_idx < 6;
16488 cartesian_trajectory_planner__B.b_idx++) {
16489 cartesian__computeProfileParams(static_cast<real_T>
16490 (cartesian_trajectory_planner__B.b_idx) + 1.0, wayPoints,
16491 cartesian_trajectory_planner__B.vel,
16492 cartesian_trajectory_planner__B.matrixInput_data,
16493 &cartesian_trajectory_planner__B.acc,
16494 &cartesian_trajectory_planner__B.segAcc,
16495 &cartesian_trajectory_planner__B.segATime,
16496 &cartesian_trajectory_planner__B.segFTime);
16497 cartesian_trajectory_planner__B.parameterMat[cartesian_trajectory_planner__B.b_idx]
16498 = wayPoints[cartesian_trajectory_planner__B.b_idx];
16499 cartesian_trajectory_planner__B.parameterMat[cartesian_trajectory_planner__B.b_idx
16500 + 6] = wayPoints[cartesian_trajectory_planner__B.b_idx + 6];
16501 cartesian_trajectory_planner__B.parameterMat[cartesian_trajectory_planner__B.b_idx
16502 + 12] = cartesian_trajectory_planner__B.acc;
16503 cartesian_trajectory_planner__B.parameterMat[cartesian_trajectory_planner__B.b_idx
16504 + 18] = cartesian_trajectory_planner__B.segAcc;
16505 cartesian_trajectory_planner__B.parameterMat[cartesian_trajectory_planner__B.b_idx
16506 + 24] = cartesian_trajectory_planner__B.segATime;
16507 cartesian_trajectory_planner__B.parameterMat[cartesian_trajectory_planner__B.b_idx
16508 + 30] = cartesian_trajectory_planner__B.segFTime;
16509 c_computeScalarLSPBCoefficients
16510 (wayPoints[cartesian_trajectory_planner__B.b_idx],
16511 wayPoints[cartesian_trajectory_planner__B.b_idx + 6],
16512 cartesian_trajectory_planner__B.acc,
16513 cartesian_trajectory_planner__B.segAcc,
16514 cartesian_trajectory_planner__B.segATime,
16515 cartesian_trajectory_planner__B.segFTime,
16516 cartesian_trajectory_planner__B.coefs,
16517 cartesian_trajectory_planner__B.breaks);
16518 for (cartesian_trajectory_planner__B.i_i = 0;
16519 cartesian_trajectory_planner__B.i_i < 18;
16520 cartesian_trajectory_planner__B.i_i++) {
16521 cartesian_trajectory_planner__B.coefIndex[cartesian_trajectory_planner__B.i_i]
16522 = false;
16523 }
16524
16525 cartesian_trajectory_planner__B.i_i = cartesian_trajectory_planner__B.b_idx
16526 + 1;
16527 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1_tmp = static_cast<
16528 int32_T>(floor(static_cast<real_T>((cartesian_trajectory_planner__B.b_idx
16529 - cartesian_trajectory_planner__B.i_i) + 13) / 6.0));
16530 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1 =
16531 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1_tmp + 1;
16532 for (cartesian_trajectory_planner__B.indivPolyDim = 0;
16533 cartesian_trajectory_planner__B.indivPolyDim <=
16534 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1_tmp;
16535 cartesian_trajectory_planner__B.indivPolyDim++) {
16536 cartesian_trajectory_planner__B.lspbSegIndices_data[cartesian_trajectory_planner__B.indivPolyDim]
16537 = 6 * cartesian_trajectory_planner__B.indivPolyDim +
16538 cartesian_trajectory_planner__B.i_i;
16539 }
16540
16541 cartesian_trajectory_planner__B.indivPolyDim =
16542 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1 - 1;
16543 for (cartesian_trajectory_planner__B.i_i = 0;
16544 cartesian_trajectory_planner__B.i_i <=
16545 cartesian_trajectory_planner__B.indivPolyDim;
16546 cartesian_trajectory_planner__B.i_i++) {
16547 cartesian_trajectory_planner__B.g_data[cartesian_trajectory_planner__B.i_i]
16548 = static_cast<int32_T>
16549 (cartesian_trajectory_planner__B.lspbSegIndices_data[cartesian_trajectory_planner__B.i_i]);
16550 }
16551
16552 for (cartesian_trajectory_planner__B.i_i = 0;
16553 cartesian_trajectory_planner__B.i_i <
16554 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1;
16555 cartesian_trajectory_planner__B.i_i++) {
16556 cartesian_trajectory_planner__B.coefIndex[cartesian_trajectory_planner__B.g_data
16557 [cartesian_trajectory_planner__B.i_i] - 1] = true;
16558 }
16559
16560 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1 = 0;
16561 cartesian_trajectory_planner__B.indivPolyDim = 0;
16562 for (cartesian_trajectory_planner__B.i_i = 0;
16563 cartesian_trajectory_planner__B.i_i < 18;
16564 cartesian_trajectory_planner__B.i_i++) {
16565 if (cartesian_trajectory_planner__B.coefIndex[cartesian_trajectory_planner__B.i_i])
16566 {
16567 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1++;
16568 cartesian_trajectory_planner__B.o_data[cartesian_trajectory_planner__B.indivPolyDim]
16569 = static_cast<int8_T>(cartesian_trajectory_planner__B.i_i + 1);
16570 cartesian_trajectory_planner__B.indivPolyDim++;
16571 }
16572 }
16573
16574 for (cartesian_trajectory_planner__B.i_i = 0;
16575 cartesian_trajectory_planner__B.i_i < 3;
16576 cartesian_trajectory_planner__B.i_i++) {
16577 for (cartesian_trajectory_planner__B.indivPolyDim = 0;
16578 cartesian_trajectory_planner__B.indivPolyDim <
16579 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1;
16580 cartesian_trajectory_planner__B.indivPolyDim++) {
16581 cartesian_trajectory_planner__B.coeffMat
16582 [(cartesian_trajectory_planner__B.o_data[cartesian_trajectory_planner__B.indivPolyDim]
16583 + 18 * cartesian_trajectory_planner__B.i_i) - 1] =
16584 cartesian_trajectory_planner__B.coefs[cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1
16585 * cartesian_trajectory_planner__B.i_i +
16586 cartesian_trajectory_planner__B.indivPolyDim];
16587 }
16588 }
16589
16590 cartesian_trajectory_planner__B.acc =
16591 cartesian_trajectory_planner__B.breakMat[cartesian_trajectory_planner__B.b_idx];
16592 cartesian_trajectory_planner__B.breakMat[cartesian_trajectory_planner__B.b_idx]
16593 += cartesian_trajectory_planner__B.breaks[0];
16594 cartesian_trajectory_planner__B.breakMat[cartesian_trajectory_planner__B.b_idx
16595 + 6] = cartesian_trajectory_planner__B.breaks[1] +
16596 cartesian_trajectory_planner__B.acc;
16597 cartesian_trajectory_planner__B.breakMat[cartesian_trajectory_planner__B.b_idx
16598 + 12] = cartesian_trajectory_planner__B.breaks[2] +
16599 cartesian_trajectory_planner__B.acc;
16600 cartesian_trajectory_planner__B.breakMat[cartesian_trajectory_planner__B.b_idx
16601 + 18] = cartesian_trajectory_planner__B.breaks[3] +
16602 cartesian_trajectory_planner__B.acc;
16603 cartesian_trajectory_planner__B.a[cartesian_trajectory_planner__B.b_idx] =
16604 cartesian_trajectory_planner__B.parameterMat[cartesian_trajectory_planner__B.b_idx
16605 + 30];
16606 }
16607
16608 cartesian_trajectory_planner__B.hasMultipleBreaks =
16609 cart_checkPolyForMultipleBreaks(cartesian_trajectory_planner__B.breakMat);
16610 cartes_processPolynomialResults(cartesian_trajectory_planner__B.breakMat,
16611 cartesian_trajectory_planner__B.coeffMat,
16612 cartesian_trajectory_planner__B.hasMultipleBreaks,
16613 cartesian_trajectory_planner__B.breaksCell,
16614 cartesian_trajectory_planner__B.coeffsCell);
16615 if (!rtIsNaN(cartesian_trajectory_planner__B.a[0])) {
16616 cartesian_trajectory_planner__B.b_idx = 1;
16617 } else {
16618 cartesian_trajectory_planner__B.b_idx = 0;
16619 cartesian_trajectory_planner__B.i_i = 2;
16620 exitg1 = false;
16621 while ((!exitg1) && (cartesian_trajectory_planner__B.i_i < 7)) {
16622 if (!rtIsNaN
16623 (cartesian_trajectory_planner__B.a[cartesian_trajectory_planner__B.i_i
16624 - 1])) {
16625 cartesian_trajectory_planner__B.b_idx =
16626 cartesian_trajectory_planner__B.i_i;
16627 exitg1 = true;
16628 } else {
16629 cartesian_trajectory_planner__B.i_i++;
16630 }
16631 }
16632 }
16633
16634 if (cartesian_trajectory_planner__B.b_idx == 0) {
16635 t[1] = cartesian_trajectory_planner__B.a[0];
16636 } else {
16637 cartesian_trajectory_planner__B.segAcc =
16638 cartesian_trajectory_planner__B.a[cartesian_trajectory_planner__B.b_idx -
16639 1];
16640 for (cartesian_trajectory_planner__B.i_i =
16641 cartesian_trajectory_planner__B.b_idx + 1;
16642 cartesian_trajectory_planner__B.i_i < 7;
16643 cartesian_trajectory_planner__B.i_i++) {
16644 cartesian_trajectory_planner__B.acc =
16645 cartesian_trajectory_planner__B.a[cartesian_trajectory_planner__B.i_i -
16646 1];
16647 if (cartesian_trajectory_planner__B.segAcc <
16648 cartesian_trajectory_planner__B.acc) {
16649 cartesian_trajectory_planner__B.segAcc =
16650 cartesian_trajectory_planner__B.acc;
16651 }
16652 }
16653
16654 t[1] = cartesian_trajectory_planner__B.segAcc;
16655 }
16656
16657 t[0] = 0.0;
16658 if (cartesian_trajectory_planner__B.hasMultipleBreaks) {
16659 cartesian_trajectory_planner__B.i_i = 6;
16660 cartesian_trajectory_planner__B.indivPolyDim = 1;
16661 } else {
16662 cartesian_trajectory_planner__B.i_i = 1;
16663 cartesian_trajectory_planner__B.indivPolyDim = 6;
16664 }
16665
16666 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1 =
16667 cartesian_trajectory_planner__B.i_i - 1;
16668 *ppCell_size = cartesian_trajectory_planner__B.i_i;
16669 for (cartesian_trajectory_planner__B.b_idx = 0;
16670 cartesian_trajectory_planner__B.b_idx <=
16671 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1;
16672 cartesian_trajectory_planner__B.b_idx++) {
16673 if (cartesian_trajectory_planner__B.hasMultipleBreaks) {
16674 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1_tmp = 1;
16675 cartesian_trajectory_planner__B.rowSelection_data[0] = static_cast<int8_T>
16676 (cartesian_trajectory_planner__B.b_idx + 1);
16677 cartesian_trajectory_planner__B.i_i =
16678 cartesian_trajectory_planner__B.b_idx + 1;
16679 } else {
16680 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1_tmp = 6;
16681 for (cartesian_trajectory_planner__B.i_i = 0;
16682 cartesian_trajectory_planner__B.i_i < 6;
16683 cartesian_trajectory_planner__B.i_i++) {
16684 cartesian_trajectory_planner__B.rowSelection_data[cartesian_trajectory_planner__B.i_i]
16685 = static_cast<int8_T>(cartesian_trajectory_planner__B.i_i + 1);
16686 }
16687
16688 cartesian_trajectory_planner__B.i_i = 1;
16689 }
16690
16691 c_generateTrajectoriesFromCoefs
16692 (cartesian_trajectory_planner__B.breaksCell[cartesian_trajectory_planner__B.i_i
16693 - 1].f1.data,
16694 cartesian_trajectory_planner__B.breaksCell[cartesian_trajectory_planner__B.i_i
16695 - 1].f1.size,
16696 cartesian_trajectory_planner__B.coeffsCell[cartesian_trajectory_planner__B.i_i
16697 - 1].f1.data,
16698 cartesian_trajectory_planner__B.coeffsCell[cartesian_trajectory_planner__B.i_i
16699 - 1].f1.size, static_cast<real_T>
16700 (cartesian_trajectory_planner__B.indivPolyDim), t,
16701 cartesian_trajectory_planner__B.b_data_cv,
16702 cartesian_trajectory_planner__B.b_size,
16703 cartesian_trajectory_planner__B.c_data,
16704 cartesian_trajectory_planner__B.c_size,
16705 cartesian_trajectory_planner__B.d_data,
16706 cartesian_trajectory_planner__B.d_size,
16707 ppCell_data[cartesian_trajectory_planner__B.b_idx].breaks.data,
16708 ppCell_data[cartesian_trajectory_planner__B.b_idx].breaks.size,
16709 ppCell_data[cartesian_trajectory_planner__B.b_idx].coefs.data,
16710 ppCell_data[cartesian_trajectory_planner__B.b_idx].coefs.size);
16711 for (cartesian_trajectory_planner__B.i_i = 0;
16712 cartesian_trajectory_planner__B.i_i <
16713 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1_tmp;
16714 cartesian_trajectory_planner__B.i_i++) {
16715 cartesian_trajectory_planner__B.l_tmp_data[cartesian_trajectory_planner__B.i_i]
16716 = static_cast<int8_T>
16717 (cartesian_trajectory_planner__B.rowSelection_data[cartesian_trajectory_planner__B.i_i]
16718 - 1);
16719 }
16720
16721 for (cartesian_trajectory_planner__B.i_i = 0;
16722 cartesian_trajectory_planner__B.i_i <
16723 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1_tmp;
16724 cartesian_trajectory_planner__B.i_i++) {
16725 q[cartesian_trajectory_planner__B.l_tmp_data[cartesian_trajectory_planner__B.i_i]]
16726 =
16727 cartesian_trajectory_planner__B.b_data_cv[cartesian_trajectory_planner__B.i_i];
16728 }
16729
16730 for (cartesian_trajectory_planner__B.i_i = 0;
16731 cartesian_trajectory_planner__B.i_i <
16732 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1_tmp;
16733 cartesian_trajectory_planner__B.i_i++) {
16734 q[cartesian_trajectory_planner__B.l_tmp_data[cartesian_trajectory_planner__B.i_i]
16735 + 6] =
16736 cartesian_trajectory_planner__B.b_data_cv[cartesian_trajectory_planner__B.i_i
16737 + cartesian_trajectory_planner__B.b_size[0]];
16738 }
16739
16740 for (cartesian_trajectory_planner__B.i_i = 0;
16741 cartesian_trajectory_planner__B.i_i <
16742 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1_tmp;
16743 cartesian_trajectory_planner__B.i_i++) {
16744 qd[cartesian_trajectory_planner__B.l_tmp_data[cartesian_trajectory_planner__B.i_i]]
16745 =
16746 cartesian_trajectory_planner__B.c_data[cartesian_trajectory_planner__B.i_i];
16747 }
16748
16749 for (cartesian_trajectory_planner__B.i_i = 0;
16750 cartesian_trajectory_planner__B.i_i <
16751 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1_tmp;
16752 cartesian_trajectory_planner__B.i_i++) {
16753 qd[cartesian_trajectory_planner__B.l_tmp_data[cartesian_trajectory_planner__B.i_i]
16754 + 6] =
16755 cartesian_trajectory_planner__B.c_data[cartesian_trajectory_planner__B.i_i
16756 + cartesian_trajectory_planner__B.c_size[0]];
16757 }
16758
16759 for (cartesian_trajectory_planner__B.i_i = 0;
16760 cartesian_trajectory_planner__B.i_i <
16761 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1_tmp;
16762 cartesian_trajectory_planner__B.i_i++) {
16763 qdd[cartesian_trajectory_planner__B.l_tmp_data[cartesian_trajectory_planner__B.i_i]]
16764 =
16765 cartesian_trajectory_planner__B.d_data[cartesian_trajectory_planner__B.i_i];
16766 }
16767
16768 for (cartesian_trajectory_planner__B.i_i = 0;
16769 cartesian_trajectory_planner__B.i_i <
16770 cartesian_trajectory_planner__B.lspbSegIndices_size_idx_1_tmp;
16771 cartesian_trajectory_planner__B.i_i++) {
16772 qdd[cartesian_trajectory_planner__B.l_tmp_data[cartesian_trajectory_planner__B.i_i]
16773 + 6] =
16774 cartesian_trajectory_planner__B.d_data[cartesian_trajectory_planner__B.i_i
16775 + cartesian_trajectory_planner__B.d_size[0]];
16776 }
16777 }
16778}
16779
16780static void TrapVelTrajSys_extract1DimFromP(const real_T pp_breaks_data[], const
16781 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], const int32_T
16782 pp_coefs_size[3], real_T breaks_data[], int32_T breaks_size[2], real_T
16783 oneDimCoeffs_data[], int32_T oneDimCoeffs_size[2])
16784{
16785 static const boolean_T tmp[6] = { true, false, false, false, false, false };
16786
16787 breaks_size[0] = 1;
16788 breaks_size[1] = pp_breaks_size[1];
16789 cartesian_trajectory_planner__B.loop_ub_l = pp_breaks_size[1];
16790 if (0 <= cartesian_trajectory_planner__B.loop_ub_l - 1) {
16791 memcpy(&breaks_data[0], &pp_breaks_data[0],
16792 cartesian_trajectory_planner__B.loop_ub_l * sizeof(real_T));
16793 }
16794
16795 for (cartesian_trajectory_planner__B.i_o3 = 0;
16796 cartesian_trajectory_planner__B.i_o3 < 6;
16797 cartesian_trajectory_planner__B.i_o3++) {
16798 cartesian_trajectory_planner__B.a_ju[cartesian_trajectory_planner__B.i_o3] =
16799 tmp[cartesian_trajectory_planner__B.i_o3];
16800 }
16801
16802 cartesian_trajectory_planner__B.ntilerows_g = pp_breaks_size[1] - 2;
16803 for (cartesian_trajectory_planner__B.loop_ub_l = 0;
16804 cartesian_trajectory_planner__B.loop_ub_l <=
16805 cartesian_trajectory_planner__B.ntilerows_g;
16806 cartesian_trajectory_planner__B.loop_ub_l++) {
16807 cartesian_trajectory_planner__B.ibcol_co =
16808 cartesian_trajectory_planner__B.loop_ub_l * 6 - 1;
16809 for (cartesian_trajectory_planner__B.i_o3 = 0;
16810 cartesian_trajectory_planner__B.i_o3 < 6;
16811 cartesian_trajectory_planner__B.i_o3++) {
16812 cartesian_trajectory_planner__B.coeffIndex_data_l
16813 [(cartesian_trajectory_planner__B.ibcol_co +
16814 cartesian_trajectory_planner__B.i_o3) + 1] =
16815 cartesian_trajectory_planner__B.a_ju[cartesian_trajectory_planner__B.i_o3];
16816 }
16817 }
16818
16819 cartesian_trajectory_planner__B.ntilerows_g = (pp_breaks_size[1] - 1) * 6 - 1;
16820 cartesian_trajectory_planner__B.loop_ub_l = 0;
16821 cartesian_trajectory_planner__B.ibcol_co = 0;
16822 for (cartesian_trajectory_planner__B.i_o3 = 0;
16823 cartesian_trajectory_planner__B.i_o3 <=
16824 cartesian_trajectory_planner__B.ntilerows_g;
16825 cartesian_trajectory_planner__B.i_o3++) {
16826 if (cartesian_trajectory_planner__B.coeffIndex_data_l[cartesian_trajectory_planner__B.i_o3])
16827 {
16828 cartesian_trajectory_planner__B.loop_ub_l++;
16829 cartesian_trajectory_planner__B.b_data_p[cartesian_trajectory_planner__B.ibcol_co]
16830 = cartesian_trajectory_planner__B.i_o3 + 1;
16831 cartesian_trajectory_planner__B.ibcol_co++;
16832 }
16833 }
16834
16835 cartesian_trajectory_planner__B.ibcol_co = div_nzp_s32(pp_coefs_size[0] *
16836 pp_coefs_size[1] * 3, 3);
16837 oneDimCoeffs_size[0] = cartesian_trajectory_planner__B.loop_ub_l;
16838 oneDimCoeffs_size[1] = 3;
16839 for (cartesian_trajectory_planner__B.i_o3 = 0;
16840 cartesian_trajectory_planner__B.i_o3 < 3;
16841 cartesian_trajectory_planner__B.i_o3++) {
16842 for (cartesian_trajectory_planner__B.ntilerows_g = 0;
16843 cartesian_trajectory_planner__B.ntilerows_g <
16844 cartesian_trajectory_planner__B.loop_ub_l;
16845 cartesian_trajectory_planner__B.ntilerows_g++) {
16846 oneDimCoeffs_data[cartesian_trajectory_planner__B.ntilerows_g +
16847 cartesian_trajectory_planner__B.loop_ub_l *
16848 cartesian_trajectory_planner__B.i_o3] = pp_coefs_data
16849 [(cartesian_trajectory_planner__B.ibcol_co *
16850 cartesian_trajectory_planner__B.i_o3 +
16851 cartesian_trajectory_planner__B.b_data_p[cartesian_trajectory_planner__B.ntilerows_g])
16852 - 1];
16853 }
16854 }
16855}
16856
16857static void cartesian_trajectory_pl_ppval_j(const real_T pp_breaks_data[], const
16858 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], real_T x, real_T
16859 v_data[], int32_T *v_size)
16860{
16861 *v_size = 1;
16862 if (rtIsNaN(x)) {
16863 v_data[0] = x;
16864 } else {
16865 cartesian_trajectory_planner__B.ip = cartesian_trajectory_pl_bsearch
16866 (pp_breaks_data, pp_breaks_size, x) - 1;
16867 cartesian_trajectory_planner__B.xloc = x -
16868 pp_breaks_data[cartesian_trajectory_planner__B.ip];
16869 v_data[0] = (pp_coefs_data[(cartesian_trajectory_planner__B.ip +
16870 pp_breaks_size[1]) - 1] + cartesian_trajectory_planner__B.xloc *
16871 pp_coefs_data[cartesian_trajectory_planner__B.ip]) *
16872 cartesian_trajectory_planner__B.xloc + pp_coefs_data[((pp_breaks_size[1] -
16873 1) << 1) + cartesian_trajectory_planner__B.ip];
16874 }
16875}
16876
16877static void TrapVelTrajSys_generate1DTrajec(const real_T breaks_data[], const
16878 int32_T breaks_size[2], const real_T coefs_data[], const int32_T coefs_size[2],
16879 real_T t, real_T q_data[], int32_T *q_size, real_T qd_data[], int32_T *qd_size,
16880 real_T qdd_data[], int32_T *qdd_size)
16881{
16882 cartesian__polyCoeffsDerivative(coefs_data, coefs_size,
16883 cartesian_trajectory_planner__B.dCoefs_data,
16884 cartesian_trajectory_planner__B.dCoefs_size);
16885 cartesian_trajectory_planner__B.breaks_size_h[0] = 1;
16886 cartesian_trajectory_planner__B.breaks_size_h[1] = breaks_size[1];
16887 cartesian_trajectory_planner__B.loop_ub_m3 = breaks_size[1];
16888 if (0 <= cartesian_trajectory_planner__B.loop_ub_m3 - 1) {
16889 memcpy(&cartesian_trajectory_planner__B.breaks_data_g[0], &breaks_data[0],
16890 cartesian_trajectory_planner__B.loop_ub_m3 * sizeof(real_T));
16891 }
16892
16893 cartesian_trajectory_pl_ppval_j(cartesian_trajectory_planner__B.breaks_data_g,
16894 cartesian_trajectory_planner__B.breaks_size_h, coefs_data, t, q_data, q_size);
16895 cartesian_trajectory_planner__B.breaks_size_b[0] = 1;
16896 cartesian_trajectory_planner__B.breaks_size_b[1] = breaks_size[1];
16897 cartesian_trajectory_planner__B.loop_ub_m3 = breaks_size[1];
16898 if (0 <= cartesian_trajectory_planner__B.loop_ub_m3 - 1) {
16899 memcpy(&cartesian_trajectory_planner__B.breaks_data_g[0], &breaks_data[0],
16900 cartesian_trajectory_planner__B.loop_ub_m3 * sizeof(real_T));
16901 }
16902
16903 cartesian_trajectory_pl_ppval_j(cartesian_trajectory_planner__B.breaks_data_g,
16904 cartesian_trajectory_planner__B.breaks_size_b,
16905 cartesian_trajectory_planner__B.dCoefs_data, t, qd_data, qd_size);
16906 cartesian__polyCoeffsDerivative(cartesian_trajectory_planner__B.dCoefs_data,
16907 cartesian_trajectory_planner__B.dCoefs_size,
16908 cartesian_trajectory_planner__B.tmp_data_m,
16909 cartesian_trajectory_planner__B.breaks_size_h);
16910 cartesian_trajectory_planner__B.breaks_size_d[0] = 1;
16911 cartesian_trajectory_planner__B.breaks_size_d[1] = breaks_size[1];
16912 cartesian_trajectory_planner__B.loop_ub_m3 = breaks_size[1];
16913 if (0 <= cartesian_trajectory_planner__B.loop_ub_m3 - 1) {
16914 memcpy(&cartesian_trajectory_planner__B.breaks_data_g[0], &breaks_data[0],
16915 cartesian_trajectory_planner__B.loop_ub_m3 * sizeof(real_T));
16916 }
16917
16918 cartesian_trajectory_pl_ppval_j(cartesian_trajectory_planner__B.breaks_data_g,
16919 cartesian_trajectory_planner__B.breaks_size_d,
16920 cartesian_trajectory_planner__B.tmp_data_m, t, qdd_data, qdd_size);
16921}
16922
16923static void TrapVelTrajSys_extract1DimFro_j(const real_T pp_breaks_data[], const
16924 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], const int32_T
16925 pp_coefs_size[3], real_T breaks_data[], int32_T breaks_size[2], real_T
16926 oneDimCoeffs_data[], int32_T oneDimCoeffs_size[2])
16927{
16928 static const boolean_T tmp[6] = { false, true, false, false, false, false };
16929
16930 breaks_size[0] = 1;
16931 breaks_size[1] = pp_breaks_size[1];
16932 cartesian_trajectory_planner__B.loop_ub_f = pp_breaks_size[1];
16933 if (0 <= cartesian_trajectory_planner__B.loop_ub_f - 1) {
16934 memcpy(&breaks_data[0], &pp_breaks_data[0],
16935 cartesian_trajectory_planner__B.loop_ub_f * sizeof(real_T));
16936 }
16937
16938 for (cartesian_trajectory_planner__B.i_iz = 0;
16939 cartesian_trajectory_planner__B.i_iz < 6;
16940 cartesian_trajectory_planner__B.i_iz++) {
16941 cartesian_trajectory_planner__B.a_a[cartesian_trajectory_planner__B.i_iz] =
16942 tmp[cartesian_trajectory_planner__B.i_iz];
16943 }
16944
16945 cartesian_trajectory_planner__B.ntilerows_ip = pp_breaks_size[1] - 2;
16946 for (cartesian_trajectory_planner__B.loop_ub_f = 0;
16947 cartesian_trajectory_planner__B.loop_ub_f <=
16948 cartesian_trajectory_planner__B.ntilerows_ip;
16949 cartesian_trajectory_planner__B.loop_ub_f++) {
16950 cartesian_trajectory_planner__B.ibcol_f =
16951 cartesian_trajectory_planner__B.loop_ub_f * 6 - 1;
16952 for (cartesian_trajectory_planner__B.i_iz = 0;
16953 cartesian_trajectory_planner__B.i_iz < 6;
16954 cartesian_trajectory_planner__B.i_iz++) {
16955 cartesian_trajectory_planner__B.coeffIndex_data_p
16956 [(cartesian_trajectory_planner__B.ibcol_f +
16957 cartesian_trajectory_planner__B.i_iz) + 1] =
16958 cartesian_trajectory_planner__B.a_a[cartesian_trajectory_planner__B.i_iz];
16959 }
16960 }
16961
16962 cartesian_trajectory_planner__B.ntilerows_ip = (pp_breaks_size[1] - 1) * 6 - 1;
16963 cartesian_trajectory_planner__B.loop_ub_f = 0;
16964 cartesian_trajectory_planner__B.ibcol_f = 0;
16965 for (cartesian_trajectory_planner__B.i_iz = 0;
16966 cartesian_trajectory_planner__B.i_iz <=
16967 cartesian_trajectory_planner__B.ntilerows_ip;
16968 cartesian_trajectory_planner__B.i_iz++) {
16969 if (cartesian_trajectory_planner__B.coeffIndex_data_p[cartesian_trajectory_planner__B.i_iz])
16970 {
16971 cartesian_trajectory_planner__B.loop_ub_f++;
16972 cartesian_trajectory_planner__B.b_data_b[cartesian_trajectory_planner__B.ibcol_f]
16973 = cartesian_trajectory_planner__B.i_iz + 1;
16974 cartesian_trajectory_planner__B.ibcol_f++;
16975 }
16976 }
16977
16978 cartesian_trajectory_planner__B.ibcol_f = div_nzp_s32(pp_coefs_size[0] *
16979 pp_coefs_size[1] * 3, 3);
16980 oneDimCoeffs_size[0] = cartesian_trajectory_planner__B.loop_ub_f;
16981 oneDimCoeffs_size[1] = 3;
16982 for (cartesian_trajectory_planner__B.i_iz = 0;
16983 cartesian_trajectory_planner__B.i_iz < 3;
16984 cartesian_trajectory_planner__B.i_iz++) {
16985 for (cartesian_trajectory_planner__B.ntilerows_ip = 0;
16986 cartesian_trajectory_planner__B.ntilerows_ip <
16987 cartesian_trajectory_planner__B.loop_ub_f;
16988 cartesian_trajectory_planner__B.ntilerows_ip++) {
16989 oneDimCoeffs_data[cartesian_trajectory_planner__B.ntilerows_ip +
16990 cartesian_trajectory_planner__B.loop_ub_f *
16991 cartesian_trajectory_planner__B.i_iz] = pp_coefs_data
16992 [(cartesian_trajectory_planner__B.ibcol_f *
16993 cartesian_trajectory_planner__B.i_iz +
16994 cartesian_trajectory_planner__B.b_data_b[cartesian_trajectory_planner__B.ntilerows_ip])
16995 - 1];
16996 }
16997 }
16998}
16999
17000static void TrapVelTrajSys_extract1DimFr_jb(const real_T pp_breaks_data[], const
17001 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], const int32_T
17002 pp_coefs_size[3], real_T breaks_data[], int32_T breaks_size[2], real_T
17003 oneDimCoeffs_data[], int32_T oneDimCoeffs_size[2])
17004{
17005 static const boolean_T tmp[6] = { false, false, true, false, false, false };
17006
17007 breaks_size[0] = 1;
17008 breaks_size[1] = pp_breaks_size[1];
17009 cartesian_trajectory_planner__B.loop_ub_o2 = pp_breaks_size[1];
17010 if (0 <= cartesian_trajectory_planner__B.loop_ub_o2 - 1) {
17011 memcpy(&breaks_data[0], &pp_breaks_data[0],
17012 cartesian_trajectory_planner__B.loop_ub_o2 * sizeof(real_T));
17013 }
17014
17015 for (cartesian_trajectory_planner__B.i_o = 0;
17016 cartesian_trajectory_planner__B.i_o < 6;
17017 cartesian_trajectory_planner__B.i_o++) {
17018 cartesian_trajectory_planner__B.a_f[cartesian_trajectory_planner__B.i_o] =
17019 tmp[cartesian_trajectory_planner__B.i_o];
17020 }
17021
17022 cartesian_trajectory_planner__B.ntilerows_i = pp_breaks_size[1] - 2;
17023 for (cartesian_trajectory_planner__B.loop_ub_o2 = 0;
17024 cartesian_trajectory_planner__B.loop_ub_o2 <=
17025 cartesian_trajectory_planner__B.ntilerows_i;
17026 cartesian_trajectory_planner__B.loop_ub_o2++) {
17027 cartesian_trajectory_planner__B.ibcol_l =
17028 cartesian_trajectory_planner__B.loop_ub_o2 * 6 - 1;
17029 for (cartesian_trajectory_planner__B.i_o = 0;
17030 cartesian_trajectory_planner__B.i_o < 6;
17031 cartesian_trajectory_planner__B.i_o++) {
17032 cartesian_trajectory_planner__B.coeffIndex_data_n
17033 [(cartesian_trajectory_planner__B.ibcol_l +
17034 cartesian_trajectory_planner__B.i_o) + 1] =
17035 cartesian_trajectory_planner__B.a_f[cartesian_trajectory_planner__B.i_o];
17036 }
17037 }
17038
17039 cartesian_trajectory_planner__B.ntilerows_i = (pp_breaks_size[1] - 1) * 6 - 1;
17040 cartesian_trajectory_planner__B.loop_ub_o2 = 0;
17041 cartesian_trajectory_planner__B.ibcol_l = 0;
17042 for (cartesian_trajectory_planner__B.i_o = 0;
17043 cartesian_trajectory_planner__B.i_o <=
17044 cartesian_trajectory_planner__B.ntilerows_i;
17045 cartesian_trajectory_planner__B.i_o++) {
17046 if (cartesian_trajectory_planner__B.coeffIndex_data_n[cartesian_trajectory_planner__B.i_o])
17047 {
17048 cartesian_trajectory_planner__B.loop_ub_o2++;
17049 cartesian_trajectory_planner__B.b_data_cx[cartesian_trajectory_planner__B.ibcol_l]
17050 = cartesian_trajectory_planner__B.i_o + 1;
17051 cartesian_trajectory_planner__B.ibcol_l++;
17052 }
17053 }
17054
17055 cartesian_trajectory_planner__B.ibcol_l = div_nzp_s32(pp_coefs_size[0] *
17056 pp_coefs_size[1] * 3, 3);
17057 oneDimCoeffs_size[0] = cartesian_trajectory_planner__B.loop_ub_o2;
17058 oneDimCoeffs_size[1] = 3;
17059 for (cartesian_trajectory_planner__B.i_o = 0;
17060 cartesian_trajectory_planner__B.i_o < 3;
17061 cartesian_trajectory_planner__B.i_o++) {
17062 for (cartesian_trajectory_planner__B.ntilerows_i = 0;
17063 cartesian_trajectory_planner__B.ntilerows_i <
17064 cartesian_trajectory_planner__B.loop_ub_o2;
17065 cartesian_trajectory_planner__B.ntilerows_i++) {
17066 oneDimCoeffs_data[cartesian_trajectory_planner__B.ntilerows_i +
17067 cartesian_trajectory_planner__B.loop_ub_o2 *
17068 cartesian_trajectory_planner__B.i_o] = pp_coefs_data
17069 [(cartesian_trajectory_planner__B.ibcol_l *
17070 cartesian_trajectory_planner__B.i_o +
17071 cartesian_trajectory_planner__B.b_data_cx[cartesian_trajectory_planner__B.ntilerows_i])
17072 - 1];
17073 }
17074 }
17075}
17076
17077static void TrapVelTrajSys_extract1DimF_jbz(const real_T pp_breaks_data[], const
17078 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], const int32_T
17079 pp_coefs_size[3], real_T breaks_data[], int32_T breaks_size[2], real_T
17080 oneDimCoeffs_data[], int32_T oneDimCoeffs_size[2])
17081{
17082 static const boolean_T tmp[6] = { false, false, false, true, false, false };
17083
17084 breaks_size[0] = 1;
17085 breaks_size[1] = pp_breaks_size[1];
17086 cartesian_trajectory_planner__B.loop_ub_a = pp_breaks_size[1];
17087 if (0 <= cartesian_trajectory_planner__B.loop_ub_a - 1) {
17088 memcpy(&breaks_data[0], &pp_breaks_data[0],
17089 cartesian_trajectory_planner__B.loop_ub_a * sizeof(real_T));
17090 }
17091
17092 for (cartesian_trajectory_planner__B.i_a = 0;
17093 cartesian_trajectory_planner__B.i_a < 6;
17094 cartesian_trajectory_planner__B.i_a++) {
17095 cartesian_trajectory_planner__B.a_j[cartesian_trajectory_planner__B.i_a] =
17096 tmp[cartesian_trajectory_planner__B.i_a];
17097 }
17098
17099 cartesian_trajectory_planner__B.ntilerows_a = pp_breaks_size[1] - 2;
17100 for (cartesian_trajectory_planner__B.loop_ub_a = 0;
17101 cartesian_trajectory_planner__B.loop_ub_a <=
17102 cartesian_trajectory_planner__B.ntilerows_a;
17103 cartesian_trajectory_planner__B.loop_ub_a++) {
17104 cartesian_trajectory_planner__B.ibcol_e =
17105 cartesian_trajectory_planner__B.loop_ub_a * 6 - 1;
17106 for (cartesian_trajectory_planner__B.i_a = 0;
17107 cartesian_trajectory_planner__B.i_a < 6;
17108 cartesian_trajectory_planner__B.i_a++) {
17109 cartesian_trajectory_planner__B.coeffIndex_data_m
17110 [(cartesian_trajectory_planner__B.ibcol_e +
17111 cartesian_trajectory_planner__B.i_a) + 1] =
17112 cartesian_trajectory_planner__B.a_j[cartesian_trajectory_planner__B.i_a];
17113 }
17114 }
17115
17116 cartesian_trajectory_planner__B.ntilerows_a = (pp_breaks_size[1] - 1) * 6 - 1;
17117 cartesian_trajectory_planner__B.loop_ub_a = 0;
17118 cartesian_trajectory_planner__B.ibcol_e = 0;
17119 for (cartesian_trajectory_planner__B.i_a = 0;
17120 cartesian_trajectory_planner__B.i_a <=
17121 cartesian_trajectory_planner__B.ntilerows_a;
17122 cartesian_trajectory_planner__B.i_a++) {
17123 if (cartesian_trajectory_planner__B.coeffIndex_data_m[cartesian_trajectory_planner__B.i_a])
17124 {
17125 cartesian_trajectory_planner__B.loop_ub_a++;
17126 cartesian_trajectory_planner__B.b_data_k[cartesian_trajectory_planner__B.ibcol_e]
17127 = cartesian_trajectory_planner__B.i_a + 1;
17128 cartesian_trajectory_planner__B.ibcol_e++;
17129 }
17130 }
17131
17132 cartesian_trajectory_planner__B.ibcol_e = div_nzp_s32(pp_coefs_size[0] *
17133 pp_coefs_size[1] * 3, 3);
17134 oneDimCoeffs_size[0] = cartesian_trajectory_planner__B.loop_ub_a;
17135 oneDimCoeffs_size[1] = 3;
17136 for (cartesian_trajectory_planner__B.i_a = 0;
17137 cartesian_trajectory_planner__B.i_a < 3;
17138 cartesian_trajectory_planner__B.i_a++) {
17139 for (cartesian_trajectory_planner__B.ntilerows_a = 0;
17140 cartesian_trajectory_planner__B.ntilerows_a <
17141 cartesian_trajectory_planner__B.loop_ub_a;
17142 cartesian_trajectory_planner__B.ntilerows_a++) {
17143 oneDimCoeffs_data[cartesian_trajectory_planner__B.ntilerows_a +
17144 cartesian_trajectory_planner__B.loop_ub_a *
17145 cartesian_trajectory_planner__B.i_a] = pp_coefs_data
17146 [(cartesian_trajectory_planner__B.ibcol_e *
17147 cartesian_trajectory_planner__B.i_a +
17148 cartesian_trajectory_planner__B.b_data_k[cartesian_trajectory_planner__B.ntilerows_a])
17149 - 1];
17150 }
17151 }
17152}
17153
17154static void TrapVelTrajSys_extract1Dim_jbzp(const real_T pp_breaks_data[], const
17155 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], const int32_T
17156 pp_coefs_size[3], real_T breaks_data[], int32_T breaks_size[2], real_T
17157 oneDimCoeffs_data[], int32_T oneDimCoeffs_size[2])
17158{
17159 static const boolean_T tmp[6] = { false, false, false, false, true, false };
17160
17161 breaks_size[0] = 1;
17162 breaks_size[1] = pp_breaks_size[1];
17163 cartesian_trajectory_planner__B.loop_ub_p = pp_breaks_size[1];
17164 if (0 <= cartesian_trajectory_planner__B.loop_ub_p - 1) {
17165 memcpy(&breaks_data[0], &pp_breaks_data[0],
17166 cartesian_trajectory_planner__B.loop_ub_p * sizeof(real_T));
17167 }
17168
17169 for (cartesian_trajectory_planner__B.i_p = 0;
17170 cartesian_trajectory_planner__B.i_p < 6;
17171 cartesian_trajectory_planner__B.i_p++) {
17172 cartesian_trajectory_planner__B.a_b[cartesian_trajectory_planner__B.i_p] =
17173 tmp[cartesian_trajectory_planner__B.i_p];
17174 }
17175
17176 cartesian_trajectory_planner__B.ntilerows_c = pp_breaks_size[1] - 2;
17177 for (cartesian_trajectory_planner__B.loop_ub_p = 0;
17178 cartesian_trajectory_planner__B.loop_ub_p <=
17179 cartesian_trajectory_planner__B.ntilerows_c;
17180 cartesian_trajectory_planner__B.loop_ub_p++) {
17181 cartesian_trajectory_planner__B.ibcol_c =
17182 cartesian_trajectory_planner__B.loop_ub_p * 6 - 1;
17183 for (cartesian_trajectory_planner__B.i_p = 0;
17184 cartesian_trajectory_planner__B.i_p < 6;
17185 cartesian_trajectory_planner__B.i_p++) {
17186 cartesian_trajectory_planner__B.coeffIndex_data_g
17187 [(cartesian_trajectory_planner__B.ibcol_c +
17188 cartesian_trajectory_planner__B.i_p) + 1] =
17189 cartesian_trajectory_planner__B.a_b[cartesian_trajectory_planner__B.i_p];
17190 }
17191 }
17192
17193 cartesian_trajectory_planner__B.ntilerows_c = (pp_breaks_size[1] - 1) * 6 - 1;
17194 cartesian_trajectory_planner__B.loop_ub_p = 0;
17195 cartesian_trajectory_planner__B.ibcol_c = 0;
17196 for (cartesian_trajectory_planner__B.i_p = 0;
17197 cartesian_trajectory_planner__B.i_p <=
17198 cartesian_trajectory_planner__B.ntilerows_c;
17199 cartesian_trajectory_planner__B.i_p++) {
17200 if (cartesian_trajectory_planner__B.coeffIndex_data_g[cartesian_trajectory_planner__B.i_p])
17201 {
17202 cartesian_trajectory_planner__B.loop_ub_p++;
17203 cartesian_trajectory_planner__B.b_data_c[cartesian_trajectory_planner__B.ibcol_c]
17204 = cartesian_trajectory_planner__B.i_p + 1;
17205 cartesian_trajectory_planner__B.ibcol_c++;
17206 }
17207 }
17208
17209 cartesian_trajectory_planner__B.ibcol_c = div_nzp_s32(pp_coefs_size[0] *
17210 pp_coefs_size[1] * 3, 3);
17211 oneDimCoeffs_size[0] = cartesian_trajectory_planner__B.loop_ub_p;
17212 oneDimCoeffs_size[1] = 3;
17213 for (cartesian_trajectory_planner__B.i_p = 0;
17214 cartesian_trajectory_planner__B.i_p < 3;
17215 cartesian_trajectory_planner__B.i_p++) {
17216 for (cartesian_trajectory_planner__B.ntilerows_c = 0;
17217 cartesian_trajectory_planner__B.ntilerows_c <
17218 cartesian_trajectory_planner__B.loop_ub_p;
17219 cartesian_trajectory_planner__B.ntilerows_c++) {
17220 oneDimCoeffs_data[cartesian_trajectory_planner__B.ntilerows_c +
17221 cartesian_trajectory_planner__B.loop_ub_p *
17222 cartesian_trajectory_planner__B.i_p] = pp_coefs_data
17223 [(cartesian_trajectory_planner__B.ibcol_c *
17224 cartesian_trajectory_planner__B.i_p +
17225 cartesian_trajectory_planner__B.b_data_c[cartesian_trajectory_planner__B.ntilerows_c])
17226 - 1];
17227 }
17228 }
17229}
17230
17231static void TrapVelTrajSys_extract1Di_jbzpa(const real_T pp_breaks_data[], const
17232 int32_T pp_breaks_size[2], const real_T pp_coefs_data[], const int32_T
17233 pp_coefs_size[3], real_T breaks_data[], int32_T breaks_size[2], real_T
17234 oneDimCoeffs_data[], int32_T oneDimCoeffs_size[2])
17235{
17236 static const boolean_T tmp[6] = { false, false, false, false, false, true };
17237
17238 breaks_size[0] = 1;
17239 breaks_size[1] = pp_breaks_size[1];
17240 cartesian_trajectory_planner__B.loop_ub_h = pp_breaks_size[1];
17241 if (0 <= cartesian_trajectory_planner__B.loop_ub_h - 1) {
17242 memcpy(&breaks_data[0], &pp_breaks_data[0],
17243 cartesian_trajectory_planner__B.loop_ub_h * sizeof(real_T));
17244 }
17245
17246 for (cartesian_trajectory_planner__B.i_j = 0;
17247 cartesian_trajectory_planner__B.i_j < 6;
17248 cartesian_trajectory_planner__B.i_j++) {
17249 cartesian_trajectory_planner__B.a_e[cartesian_trajectory_planner__B.i_j] =
17250 tmp[cartesian_trajectory_planner__B.i_j];
17251 }
17252
17253 cartesian_trajectory_planner__B.ntilerows = pp_breaks_size[1] - 2;
17254 for (cartesian_trajectory_planner__B.loop_ub_h = 0;
17255 cartesian_trajectory_planner__B.loop_ub_h <=
17256 cartesian_trajectory_planner__B.ntilerows;
17257 cartesian_trajectory_planner__B.loop_ub_h++) {
17258 cartesian_trajectory_planner__B.ibcol =
17259 cartesian_trajectory_planner__B.loop_ub_h * 6 - 1;
17260 for (cartesian_trajectory_planner__B.i_j = 0;
17261 cartesian_trajectory_planner__B.i_j < 6;
17262 cartesian_trajectory_planner__B.i_j++) {
17263 cartesian_trajectory_planner__B.coeffIndex_data
17264 [(cartesian_trajectory_planner__B.ibcol +
17265 cartesian_trajectory_planner__B.i_j) + 1] =
17266 cartesian_trajectory_planner__B.a_e[cartesian_trajectory_planner__B.i_j];
17267 }
17268 }
17269
17270 cartesian_trajectory_planner__B.ntilerows = (pp_breaks_size[1] - 1) * 6 - 1;
17271 cartesian_trajectory_planner__B.loop_ub_h = 0;
17272 cartesian_trajectory_planner__B.ibcol = 0;
17273 for (cartesian_trajectory_planner__B.i_j = 0;
17274 cartesian_trajectory_planner__B.i_j <=
17275 cartesian_trajectory_planner__B.ntilerows;
17276 cartesian_trajectory_planner__B.i_j++) {
17277 if (cartesian_trajectory_planner__B.coeffIndex_data[cartesian_trajectory_planner__B.i_j])
17278 {
17279 cartesian_trajectory_planner__B.loop_ub_h++;
17280 cartesian_trajectory_planner__B.b_data[cartesian_trajectory_planner__B.ibcol]
17281 = cartesian_trajectory_planner__B.i_j + 1;
17282 cartesian_trajectory_planner__B.ibcol++;
17283 }
17284 }
17285
17286 cartesian_trajectory_planner__B.ibcol = div_nzp_s32(pp_coefs_size[0] *
17287 pp_coefs_size[1] * 3, 3);
17288 oneDimCoeffs_size[0] = cartesian_trajectory_planner__B.loop_ub_h;
17289 oneDimCoeffs_size[1] = 3;
17290 for (cartesian_trajectory_planner__B.i_j = 0;
17291 cartesian_trajectory_planner__B.i_j < 3;
17292 cartesian_trajectory_planner__B.i_j++) {
17293 for (cartesian_trajectory_planner__B.ntilerows = 0;
17294 cartesian_trajectory_planner__B.ntilerows <
17295 cartesian_trajectory_planner__B.loop_ub_h;
17296 cartesian_trajectory_planner__B.ntilerows++) {
17297 oneDimCoeffs_data[cartesian_trajectory_planner__B.ntilerows +
17298 cartesian_trajectory_planner__B.loop_ub_h *
17299 cartesian_trajectory_planner__B.i_j] = pp_coefs_data
17300 [(cartesian_trajectory_planner__B.ibcol *
17301 cartesian_trajectory_planner__B.i_j +
17302 cartesian_trajectory_planner__B.b_data[cartesian_trajectory_planner__B.ntilerows])
17303 - 1];
17304 }
17305 }
17306}
17307
17308static void cartesi_TrapVelTrajSys_stepImpl(real_T time, const real_T
17309 varargin_1[12], const real_T varargin_2[6], real_T varargin_3, real_T q[6],
17310 real_T qd[6], real_T qdd[6])
17311{
17312 cartesian_trajector_trapveltraj(varargin_1, varargin_2, varargin_3,
17313 cartesian_trajectory_planner__B.unusedU10,
17314 cartesian_trajectory_planner__B.unusedU11,
17315 cartesian_trajectory_planner__B.unusedU12,
17316 cartesian_trajectory_planner__B.unusedU13,
17317 cartesian_trajectory_planner__B.trajPP.data,
17318 &cartesian_trajectory_planner__B.trajPP.size);
17319 if (cartesian_trajectory_planner__B.trajPP.size > 1) {
17320 cartesian_trajectory_planner__B.breaks_size[0] = 1;
17321 cartesian_trajectory_planner__B.breaks_size[1] =
17322 cartesian_trajectory_planner__B.trajPP.data[0].breaks.size[1];
17323 cartesian_trajectory_planner__B.loop_ub =
17324 cartesian_trajectory_planner__B.trajPP.data[0].breaks.size[1];
17325 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17326 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp <
17327 cartesian_trajectory_planner__B.loop_ub;
17328 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17329 cartesian_trajectory_planner__B.breaks_data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp]
17330 = cartesian_trajectory_planner__B.trajPP.data[0]
17331 .breaks.data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp];
17332 }
17333
17334 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = div_nzp_s32
17335 (cartesian_trajectory_planner__B.trajPP.data[0].coefs.size[0] *
17336 cartesian_trajectory_planner__B.trajPP.data[0].coefs.size[1] * 3, 3);
17337 cartesian_trajectory_planner__B.oneDimCoeffs_size[0] =
17338 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp;
17339 cartesian_trajectory_planner__B.loop_ub =
17340 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp * 3 - 1;
17341 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17342 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp <=
17343 cartesian_trajectory_planner__B.loop_ub;
17344 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17345 cartesian_trajectory_planner__B.oneDimCoeffs_data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp]
17346 = cartesian_trajectory_planner__B.trajPP.data[0]
17347 .coefs.data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp];
17348 }
17349 } else {
17350 TrapVelTrajSys_extract1DimFromP(cartesian_trajectory_planner__B.trajPP.data
17351 [0].breaks.data, cartesian_trajectory_planner__B.trajPP.data[0].
17352 breaks.size, cartesian_trajectory_planner__B.trajPP.data[0].coefs.data,
17353 cartesian_trajectory_planner__B.trajPP.data[0].coefs.size,
17354 cartesian_trajectory_planner__B.breaks_data,
17355 cartesian_trajectory_planner__B.breaks_size,
17356 cartesian_trajectory_planner__B.oneDimCoeffs_data,
17357 cartesian_trajectory_planner__B.oneDimCoeffs_size);
17358 }
17359
17360 cartesian_trajectory_planner__B.evalCoeffs_size[0] =
17361 cartesian_trajectory_planner__B.oneDimCoeffs_size[0];
17362 cartesian_trajectory_planner__B.evalCoeffs_size[1] = 3;
17363 cartesian_trajectory_planner__B.loop_ub =
17364 cartesian_trajectory_planner__B.oneDimCoeffs_size[0] * 3 - 1;
17365 if (0 <= cartesian_trajectory_planner__B.loop_ub) {
17366 memset(&cartesian_trajectory_planner__B.evalCoeffs_data[0], 0,
17367 (cartesian_trajectory_planner__B.loop_ub + 1) * sizeof(real_T));
17368 }
17369
17370 if (1 > cartesian_trajectory_planner__B.oneDimCoeffs_size[0]) {
17371 cartesian_trajectory_planner__B.f = 0;
17372 } else {
17373 cartesian_trajectory_planner__B.f =
17374 cartesian_trajectory_planner__B.oneDimCoeffs_size[0];
17375 }
17376
17377 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17378 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp < 3;
17379 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17380 for (cartesian_trajectory_planner__B.loop_ub = 0;
17381 cartesian_trajectory_planner__B.loop_ub <
17382 cartesian_trajectory_planner__B.f;
17383 cartesian_trajectory_planner__B.loop_ub++) {
17384 cartesian_trajectory_planner__B.evalCoeffs_data[cartesian_trajectory_planner__B.loop_ub
17385 + cartesian_trajectory_planner__B.evalCoeffs_size[0] *
17386 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp] =
17387 cartesian_trajectory_planner__B.oneDimCoeffs_data[cartesian_trajectory_planner__B.oneDimCoeffs_size
17388 [0] * cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp +
17389 cartesian_trajectory_planner__B.loop_ub];
17390 }
17391 }
17392
17393 TrapVelTrajSys_generate1DTrajec(cartesian_trajectory_planner__B.breaks_data,
17394 cartesian_trajectory_planner__B.breaks_size,
17395 cartesian_trajectory_planner__B.evalCoeffs_data,
17396 cartesian_trajectory_planner__B.evalCoeffs_size, time,
17397 &cartesian_trajectory_planner__B.d_data_d,
17398 &cartesian_trajectory_planner__B.d_size_n,
17399 &cartesian_trajectory_planner__B.c_data_l,
17400 &cartesian_trajectory_planner__B.c_size_o,
17401 &cartesian_trajectory_planner__B.b_data_g,
17402 &cartesian_trajectory_planner__B.b_size_j);
17403 if (cartesian_trajectory_planner__B.trajPP.size > 1) {
17404 cartesian_trajectory_planner__B.breaks_size[0] = 1;
17405 cartesian_trajectory_planner__B.breaks_size[1] =
17406 cartesian_trajectory_planner__B.trajPP.data[1].breaks.size[1];
17407 cartesian_trajectory_planner__B.loop_ub =
17408 cartesian_trajectory_planner__B.trajPP.data[1].breaks.size[1];
17409 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17410 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp <
17411 cartesian_trajectory_planner__B.loop_ub;
17412 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17413 cartesian_trajectory_planner__B.breaks_data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp]
17414 = cartesian_trajectory_planner__B.trajPP.data[1]
17415 .breaks.data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp];
17416 }
17417
17418 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = div_nzp_s32
17419 (cartesian_trajectory_planner__B.trajPP.data[1].coefs.size[0] *
17420 cartesian_trajectory_planner__B.trajPP.data[1].coefs.size[1] * 3, 3);
17421 cartesian_trajectory_planner__B.oneDimCoeffs_size[0] =
17422 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp;
17423 cartesian_trajectory_planner__B.loop_ub =
17424 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp * 3 - 1;
17425 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17426 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp <=
17427 cartesian_trajectory_planner__B.loop_ub;
17428 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17429 cartesian_trajectory_planner__B.oneDimCoeffs_data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp]
17430 = cartesian_trajectory_planner__B.trajPP.data[1]
17431 .coefs.data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp];
17432 }
17433 } else {
17434 TrapVelTrajSys_extract1DimFro_j(cartesian_trajectory_planner__B.trajPP.data
17435 [0].breaks.data, cartesian_trajectory_planner__B.trajPP.data[0].
17436 breaks.size, cartesian_trajectory_planner__B.trajPP.data[0].coefs.data,
17437 cartesian_trajectory_planner__B.trajPP.data[0].coefs.size,
17438 cartesian_trajectory_planner__B.breaks_data,
17439 cartesian_trajectory_planner__B.breaks_size,
17440 cartesian_trajectory_planner__B.oneDimCoeffs_data,
17441 cartesian_trajectory_planner__B.oneDimCoeffs_size);
17442 }
17443
17444 cartesian_trajectory_planner__B.evalCoeffs_size[0] =
17445 cartesian_trajectory_planner__B.oneDimCoeffs_size[0];
17446 cartesian_trajectory_planner__B.evalCoeffs_size[1] = 3;
17447 cartesian_trajectory_planner__B.loop_ub =
17448 cartesian_trajectory_planner__B.oneDimCoeffs_size[0] * 3 - 1;
17449 if (0 <= cartesian_trajectory_planner__B.loop_ub) {
17450 memset(&cartesian_trajectory_planner__B.evalCoeffs_data[0], 0,
17451 (cartesian_trajectory_planner__B.loop_ub + 1) * sizeof(real_T));
17452 }
17453
17454 if (1 > cartesian_trajectory_planner__B.oneDimCoeffs_size[0]) {
17455 cartesian_trajectory_planner__B.f = 0;
17456 } else {
17457 cartesian_trajectory_planner__B.f =
17458 cartesian_trajectory_planner__B.oneDimCoeffs_size[0];
17459 }
17460
17461 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17462 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp < 3;
17463 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17464 for (cartesian_trajectory_planner__B.loop_ub = 0;
17465 cartesian_trajectory_planner__B.loop_ub <
17466 cartesian_trajectory_planner__B.f;
17467 cartesian_trajectory_planner__B.loop_ub++) {
17468 cartesian_trajectory_planner__B.evalCoeffs_data[cartesian_trajectory_planner__B.loop_ub
17469 + cartesian_trajectory_planner__B.evalCoeffs_size[0] *
17470 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp] =
17471 cartesian_trajectory_planner__B.oneDimCoeffs_data[cartesian_trajectory_planner__B.oneDimCoeffs_size
17472 [0] * cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp +
17473 cartesian_trajectory_planner__B.loop_ub];
17474 }
17475 }
17476
17477 TrapVelTrajSys_generate1DTrajec(cartesian_trajectory_planner__B.breaks_data,
17478 cartesian_trajectory_planner__B.breaks_size,
17479 cartesian_trajectory_planner__B.evalCoeffs_data,
17480 cartesian_trajectory_planner__B.evalCoeffs_size, time,
17481 &cartesian_trajectory_planner__B.j_data,
17482 &cartesian_trajectory_planner__B.d_size_n,
17483 &cartesian_trajectory_planner__B.i_data,
17484 &cartesian_trajectory_planner__B.c_size_o,
17485 &cartesian_trajectory_planner__B.h_data,
17486 &cartesian_trajectory_planner__B.b_size_j);
17487 if (cartesian_trajectory_planner__B.trajPP.size > 1) {
17488 cartesian_trajectory_planner__B.breaks_size[0] = 1;
17489 cartesian_trajectory_planner__B.breaks_size[1] =
17490 cartesian_trajectory_planner__B.trajPP.data[2].breaks.size[1];
17491 cartesian_trajectory_planner__B.loop_ub =
17492 cartesian_trajectory_planner__B.trajPP.data[2].breaks.size[1];
17493 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17494 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp <
17495 cartesian_trajectory_planner__B.loop_ub;
17496 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17497 cartesian_trajectory_planner__B.breaks_data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp]
17498 = cartesian_trajectory_planner__B.trajPP.data[2]
17499 .breaks.data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp];
17500 }
17501
17502 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = div_nzp_s32
17503 (cartesian_trajectory_planner__B.trajPP.data[2].coefs.size[0] *
17504 cartesian_trajectory_planner__B.trajPP.data[2].coefs.size[1] * 3, 3);
17505 cartesian_trajectory_planner__B.oneDimCoeffs_size[0] =
17506 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp;
17507 cartesian_trajectory_planner__B.loop_ub =
17508 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp * 3 - 1;
17509 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17510 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp <=
17511 cartesian_trajectory_planner__B.loop_ub;
17512 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17513 cartesian_trajectory_planner__B.oneDimCoeffs_data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp]
17514 = cartesian_trajectory_planner__B.trajPP.data[2]
17515 .coefs.data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp];
17516 }
17517 } else {
17518 TrapVelTrajSys_extract1DimFr_jb(cartesian_trajectory_planner__B.trajPP.data
17519 [0].breaks.data, cartesian_trajectory_planner__B.trajPP.data[0].
17520 breaks.size, cartesian_trajectory_planner__B.trajPP.data[0].coefs.data,
17521 cartesian_trajectory_planner__B.trajPP.data[0].coefs.size,
17522 cartesian_trajectory_planner__B.breaks_data,
17523 cartesian_trajectory_planner__B.breaks_size,
17524 cartesian_trajectory_planner__B.oneDimCoeffs_data,
17525 cartesian_trajectory_planner__B.oneDimCoeffs_size);
17526 }
17527
17528 cartesian_trajectory_planner__B.evalCoeffs_size[0] =
17529 cartesian_trajectory_planner__B.oneDimCoeffs_size[0];
17530 cartesian_trajectory_planner__B.evalCoeffs_size[1] = 3;
17531 cartesian_trajectory_planner__B.loop_ub =
17532 cartesian_trajectory_planner__B.oneDimCoeffs_size[0] * 3 - 1;
17533 if (0 <= cartesian_trajectory_planner__B.loop_ub) {
17534 memset(&cartesian_trajectory_planner__B.evalCoeffs_data[0], 0,
17535 (cartesian_trajectory_planner__B.loop_ub + 1) * sizeof(real_T));
17536 }
17537
17538 if (1 > cartesian_trajectory_planner__B.oneDimCoeffs_size[0]) {
17539 cartesian_trajectory_planner__B.f = 0;
17540 } else {
17541 cartesian_trajectory_planner__B.f =
17542 cartesian_trajectory_planner__B.oneDimCoeffs_size[0];
17543 }
17544
17545 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17546 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp < 3;
17547 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17548 for (cartesian_trajectory_planner__B.loop_ub = 0;
17549 cartesian_trajectory_planner__B.loop_ub <
17550 cartesian_trajectory_planner__B.f;
17551 cartesian_trajectory_planner__B.loop_ub++) {
17552 cartesian_trajectory_planner__B.evalCoeffs_data[cartesian_trajectory_planner__B.loop_ub
17553 + cartesian_trajectory_planner__B.evalCoeffs_size[0] *
17554 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp] =
17555 cartesian_trajectory_planner__B.oneDimCoeffs_data[cartesian_trajectory_planner__B.oneDimCoeffs_size
17556 [0] * cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp +
17557 cartesian_trajectory_planner__B.loop_ub];
17558 }
17559 }
17560
17561 TrapVelTrajSys_generate1DTrajec(cartesian_trajectory_planner__B.breaks_data,
17562 cartesian_trajectory_planner__B.breaks_size,
17563 cartesian_trajectory_planner__B.evalCoeffs_data,
17564 cartesian_trajectory_planner__B.evalCoeffs_size, time,
17565 &cartesian_trajectory_planner__B.p_data,
17566 &cartesian_trajectory_planner__B.d_size_n,
17567 &cartesian_trajectory_planner__B.o_data_d,
17568 &cartesian_trajectory_planner__B.c_size_o,
17569 &cartesian_trajectory_planner__B.n_data,
17570 &cartesian_trajectory_planner__B.b_size_j);
17571 if (cartesian_trajectory_planner__B.trajPP.size > 1) {
17572 cartesian_trajectory_planner__B.breaks_size[0] = 1;
17573 cartesian_trajectory_planner__B.breaks_size[1] =
17574 cartesian_trajectory_planner__B.trajPP.data[3].breaks.size[1];
17575 cartesian_trajectory_planner__B.loop_ub =
17576 cartesian_trajectory_planner__B.trajPP.data[3].breaks.size[1];
17577 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17578 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp <
17579 cartesian_trajectory_planner__B.loop_ub;
17580 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17581 cartesian_trajectory_planner__B.breaks_data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp]
17582 = cartesian_trajectory_planner__B.trajPP.data[3]
17583 .breaks.data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp];
17584 }
17585
17586 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = div_nzp_s32
17587 (cartesian_trajectory_planner__B.trajPP.data[3].coefs.size[0] *
17588 cartesian_trajectory_planner__B.trajPP.data[3].coefs.size[1] * 3, 3);
17589 cartesian_trajectory_planner__B.oneDimCoeffs_size[0] =
17590 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp;
17591 cartesian_trajectory_planner__B.loop_ub =
17592 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp * 3 - 1;
17593 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17594 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp <=
17595 cartesian_trajectory_planner__B.loop_ub;
17596 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17597 cartesian_trajectory_planner__B.oneDimCoeffs_data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp]
17598 = cartesian_trajectory_planner__B.trajPP.data[3]
17599 .coefs.data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp];
17600 }
17601 } else {
17602 TrapVelTrajSys_extract1DimF_jbz(cartesian_trajectory_planner__B.trajPP.data
17603 [0].breaks.data, cartesian_trajectory_planner__B.trajPP.data[0].
17604 breaks.size, cartesian_trajectory_planner__B.trajPP.data[0].coefs.data,
17605 cartesian_trajectory_planner__B.trajPP.data[0].coefs.size,
17606 cartesian_trajectory_planner__B.breaks_data,
17607 cartesian_trajectory_planner__B.breaks_size,
17608 cartesian_trajectory_planner__B.oneDimCoeffs_data,
17609 cartesian_trajectory_planner__B.oneDimCoeffs_size);
17610 }
17611
17612 cartesian_trajectory_planner__B.evalCoeffs_size[0] =
17613 cartesian_trajectory_planner__B.oneDimCoeffs_size[0];
17614 cartesian_trajectory_planner__B.evalCoeffs_size[1] = 3;
17615 cartesian_trajectory_planner__B.loop_ub =
17616 cartesian_trajectory_planner__B.oneDimCoeffs_size[0] * 3 - 1;
17617 if (0 <= cartesian_trajectory_planner__B.loop_ub) {
17618 memset(&cartesian_trajectory_planner__B.evalCoeffs_data[0], 0,
17619 (cartesian_trajectory_planner__B.loop_ub + 1) * sizeof(real_T));
17620 }
17621
17622 if (1 > cartesian_trajectory_planner__B.oneDimCoeffs_size[0]) {
17623 cartesian_trajectory_planner__B.f = 0;
17624 } else {
17625 cartesian_trajectory_planner__B.f =
17626 cartesian_trajectory_planner__B.oneDimCoeffs_size[0];
17627 }
17628
17629 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17630 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp < 3;
17631 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17632 for (cartesian_trajectory_planner__B.loop_ub = 0;
17633 cartesian_trajectory_planner__B.loop_ub <
17634 cartesian_trajectory_planner__B.f;
17635 cartesian_trajectory_planner__B.loop_ub++) {
17636 cartesian_trajectory_planner__B.evalCoeffs_data[cartesian_trajectory_planner__B.loop_ub
17637 + cartesian_trajectory_planner__B.evalCoeffs_size[0] *
17638 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp] =
17639 cartesian_trajectory_planner__B.oneDimCoeffs_data[cartesian_trajectory_planner__B.oneDimCoeffs_size
17640 [0] * cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp +
17641 cartesian_trajectory_planner__B.loop_ub];
17642 }
17643 }
17644
17645 TrapVelTrajSys_generate1DTrajec(cartesian_trajectory_planner__B.breaks_data,
17646 cartesian_trajectory_planner__B.breaks_size,
17647 cartesian_trajectory_planner__B.evalCoeffs_data,
17648 cartesian_trajectory_planner__B.evalCoeffs_size, time,
17649 &cartesian_trajectory_planner__B.w_data,
17650 &cartesian_trajectory_planner__B.d_size_n,
17651 &cartesian_trajectory_planner__B.v_data,
17652 &cartesian_trajectory_planner__B.c_size_o,
17653 &cartesian_trajectory_planner__B.u_data,
17654 &cartesian_trajectory_planner__B.b_size_j);
17655 if (cartesian_trajectory_planner__B.trajPP.size > 1) {
17656 cartesian_trajectory_planner__B.breaks_size[0] = 1;
17657 cartesian_trajectory_planner__B.breaks_size[1] =
17658 cartesian_trajectory_planner__B.trajPP.data[4].breaks.size[1];
17659 cartesian_trajectory_planner__B.loop_ub =
17660 cartesian_trajectory_planner__B.trajPP.data[4].breaks.size[1];
17661 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17662 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp <
17663 cartesian_trajectory_planner__B.loop_ub;
17664 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17665 cartesian_trajectory_planner__B.breaks_data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp]
17666 = cartesian_trajectory_planner__B.trajPP.data[4]
17667 .breaks.data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp];
17668 }
17669
17670 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = div_nzp_s32
17671 (cartesian_trajectory_planner__B.trajPP.data[4].coefs.size[0] *
17672 cartesian_trajectory_planner__B.trajPP.data[4].coefs.size[1] * 3, 3);
17673 cartesian_trajectory_planner__B.oneDimCoeffs_size[0] =
17674 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp;
17675 cartesian_trajectory_planner__B.loop_ub =
17676 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp * 3 - 1;
17677 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17678 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp <=
17679 cartesian_trajectory_planner__B.loop_ub;
17680 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17681 cartesian_trajectory_planner__B.oneDimCoeffs_data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp]
17682 = cartesian_trajectory_planner__B.trajPP.data[4]
17683 .coefs.data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp];
17684 }
17685 } else {
17686 TrapVelTrajSys_extract1Dim_jbzp(cartesian_trajectory_planner__B.trajPP.data
17687 [0].breaks.data, cartesian_trajectory_planner__B.trajPP.data[0].
17688 breaks.size, cartesian_trajectory_planner__B.trajPP.data[0].coefs.data,
17689 cartesian_trajectory_planner__B.trajPP.data[0].coefs.size,
17690 cartesian_trajectory_planner__B.breaks_data,
17691 cartesian_trajectory_planner__B.breaks_size,
17692 cartesian_trajectory_planner__B.oneDimCoeffs_data,
17693 cartesian_trajectory_planner__B.oneDimCoeffs_size);
17694 }
17695
17696 cartesian_trajectory_planner__B.evalCoeffs_size[0] =
17697 cartesian_trajectory_planner__B.oneDimCoeffs_size[0];
17698 cartesian_trajectory_planner__B.evalCoeffs_size[1] = 3;
17699 cartesian_trajectory_planner__B.loop_ub =
17700 cartesian_trajectory_planner__B.oneDimCoeffs_size[0] * 3 - 1;
17701 if (0 <= cartesian_trajectory_planner__B.loop_ub) {
17702 memset(&cartesian_trajectory_planner__B.evalCoeffs_data[0], 0,
17703 (cartesian_trajectory_planner__B.loop_ub + 1) * sizeof(real_T));
17704 }
17705
17706 if (1 > cartesian_trajectory_planner__B.oneDimCoeffs_size[0]) {
17707 cartesian_trajectory_planner__B.f = 0;
17708 } else {
17709 cartesian_trajectory_planner__B.f =
17710 cartesian_trajectory_planner__B.oneDimCoeffs_size[0];
17711 }
17712
17713 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17714 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp < 3;
17715 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17716 for (cartesian_trajectory_planner__B.loop_ub = 0;
17717 cartesian_trajectory_planner__B.loop_ub <
17718 cartesian_trajectory_planner__B.f;
17719 cartesian_trajectory_planner__B.loop_ub++) {
17720 cartesian_trajectory_planner__B.evalCoeffs_data[cartesian_trajectory_planner__B.loop_ub
17721 + cartesian_trajectory_planner__B.evalCoeffs_size[0] *
17722 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp] =
17723 cartesian_trajectory_planner__B.oneDimCoeffs_data[cartesian_trajectory_planner__B.oneDimCoeffs_size
17724 [0] * cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp +
17725 cartesian_trajectory_planner__B.loop_ub];
17726 }
17727 }
17728
17729 TrapVelTrajSys_generate1DTrajec(cartesian_trajectory_planner__B.breaks_data,
17730 cartesian_trajectory_planner__B.breaks_size,
17731 cartesian_trajectory_planner__B.evalCoeffs_data,
17732 cartesian_trajectory_planner__B.evalCoeffs_size, time,
17733 &cartesian_trajectory_planner__B.db_data,
17734 &cartesian_trajectory_planner__B.d_size_n,
17735 &cartesian_trajectory_planner__B.cb_data,
17736 &cartesian_trajectory_planner__B.c_size_o,
17737 &cartesian_trajectory_planner__B.bb_data,
17738 &cartesian_trajectory_planner__B.b_size_j);
17739 if (cartesian_trajectory_planner__B.trajPP.size > 1) {
17740 cartesian_trajectory_planner__B.breaks_size[0] = 1;
17741 cartesian_trajectory_planner__B.breaks_size[1] =
17742 cartesian_trajectory_planner__B.trajPP.data[5].breaks.size[1];
17743 cartesian_trajectory_planner__B.loop_ub =
17744 cartesian_trajectory_planner__B.trajPP.data[5].breaks.size[1];
17745 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17746 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp <
17747 cartesian_trajectory_planner__B.loop_ub;
17748 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17749 cartesian_trajectory_planner__B.breaks_data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp]
17750 = cartesian_trajectory_planner__B.trajPP.data[5]
17751 .breaks.data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp];
17752 }
17753
17754 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = div_nzp_s32
17755 (cartesian_trajectory_planner__B.trajPP.data[5].coefs.size[0] *
17756 cartesian_trajectory_planner__B.trajPP.data[5].coefs.size[1] * 3, 3);
17757 cartesian_trajectory_planner__B.oneDimCoeffs_size[0] =
17758 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp;
17759 cartesian_trajectory_planner__B.loop_ub =
17760 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp * 3 - 1;
17761 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17762 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp <=
17763 cartesian_trajectory_planner__B.loop_ub;
17764 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17765 cartesian_trajectory_planner__B.oneDimCoeffs_data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp]
17766 = cartesian_trajectory_planner__B.trajPP.data[5]
17767 .coefs.data[cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp];
17768 }
17769 } else {
17770 TrapVelTrajSys_extract1Di_jbzpa(cartesian_trajectory_planner__B.trajPP.data
17771 [0].breaks.data, cartesian_trajectory_planner__B.trajPP.data[0].
17772 breaks.size, cartesian_trajectory_planner__B.trajPP.data[0].coefs.data,
17773 cartesian_trajectory_planner__B.trajPP.data[0].coefs.size,
17774 cartesian_trajectory_planner__B.breaks_data,
17775 cartesian_trajectory_planner__B.breaks_size,
17776 cartesian_trajectory_planner__B.oneDimCoeffs_data,
17777 cartesian_trajectory_planner__B.oneDimCoeffs_size);
17778 }
17779
17780 cartesian_trajectory_planner__B.evalCoeffs_size[0] =
17781 cartesian_trajectory_planner__B.oneDimCoeffs_size[0];
17782 cartesian_trajectory_planner__B.evalCoeffs_size[1] = 3;
17783 cartesian_trajectory_planner__B.loop_ub =
17784 cartesian_trajectory_planner__B.oneDimCoeffs_size[0] * 3 - 1;
17785 if (0 <= cartesian_trajectory_planner__B.loop_ub) {
17786 memset(&cartesian_trajectory_planner__B.evalCoeffs_data[0], 0,
17787 (cartesian_trajectory_planner__B.loop_ub + 1) * sizeof(real_T));
17788 }
17789
17790 if (1 > cartesian_trajectory_planner__B.oneDimCoeffs_size[0]) {
17791 cartesian_trajectory_planner__B.f = 0;
17792 } else {
17793 cartesian_trajectory_planner__B.f =
17794 cartesian_trajectory_planner__B.oneDimCoeffs_size[0];
17795 }
17796
17797 for (cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp = 0;
17798 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp < 3;
17799 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp++) {
17800 for (cartesian_trajectory_planner__B.loop_ub = 0;
17801 cartesian_trajectory_planner__B.loop_ub <
17802 cartesian_trajectory_planner__B.f;
17803 cartesian_trajectory_planner__B.loop_ub++) {
17804 cartesian_trajectory_planner__B.evalCoeffs_data[cartesian_trajectory_planner__B.loop_ub
17805 + cartesian_trajectory_planner__B.evalCoeffs_size[0] *
17806 cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp] =
17807 cartesian_trajectory_planner__B.oneDimCoeffs_data[cartesian_trajectory_planner__B.oneDimCoeffs_size
17808 [0] * cartesian_trajectory_planner__B.oneDimCoeffs_size_tmp +
17809 cartesian_trajectory_planner__B.loop_ub];
17810 }
17811 }
17812
17813 TrapVelTrajSys_generate1DTrajec(cartesian_trajectory_planner__B.breaks_data,
17814 cartesian_trajectory_planner__B.breaks_size,
17815 cartesian_trajectory_planner__B.evalCoeffs_data,
17816 cartesian_trajectory_planner__B.evalCoeffs_size, time,
17817 &cartesian_trajectory_planner__B.jb_data,
17818 &cartesian_trajectory_planner__B.d_size_n,
17819 &cartesian_trajectory_planner__B.ib_data,
17820 &cartesian_trajectory_planner__B.c_size_o,
17821 &cartesian_trajectory_planner__B.hb_data,
17822 &cartesian_trajectory_planner__B.b_size_j);
17823 q[0] = cartesian_trajectory_planner__B.d_data_d;
17824 q[1] = cartesian_trajectory_planner__B.j_data;
17825 q[2] = cartesian_trajectory_planner__B.p_data;
17826 q[3] = cartesian_trajectory_planner__B.w_data;
17827 q[4] = cartesian_trajectory_planner__B.db_data;
17828 q[5] = cartesian_trajectory_planner__B.jb_data;
17829 qd[0] = cartesian_trajectory_planner__B.c_data_l;
17830 qd[1] = cartesian_trajectory_planner__B.i_data;
17831 qd[2] = cartesian_trajectory_planner__B.o_data_d;
17832 qd[3] = cartesian_trajectory_planner__B.v_data;
17833 qd[4] = cartesian_trajectory_planner__B.cb_data;
17834 qd[5] = cartesian_trajectory_planner__B.ib_data;
17835 qdd[0] = cartesian_trajectory_planner__B.b_data_g;
17836 qdd[1] = cartesian_trajectory_planner__B.h_data;
17837 qdd[2] = cartesian_trajectory_planner__B.n_data;
17838 qdd[3] = cartesian_trajectory_planner__B.u_data;
17839 qdd[4] = cartesian_trajectory_planner__B.bb_data;
17840 qdd[5] = cartesian_trajectory_planner__B.hb_data;
17841}
17842
17843static void matlabCodegenHandle_matlabC_jbz(ros_slros_internal_block_GetP_T *obj)
17844{
17845 if (!obj->matlabCodegenIsDeleted) {
17846 obj->matlabCodegenIsDeleted = true;
17847 }
17848}
17849
17850static void matlabCodegenHandle_matlabCo_jb(ros_slros_internal_block_Subs_T *obj)
17851{
17852 if (!obj->matlabCodegenIsDeleted) {
17853 obj->matlabCodegenIsDeleted = true;
17854 }
17855}
17856
17857static void matlabCodegenHandle_matlabCod_j(ros_slros_internal_block_Publ_T *obj)
17858{
17859 if (!obj->matlabCodegenIsDeleted) {
17860 obj->matlabCodegenIsDeleted = true;
17861 }
17862}
17863
17864static void cartesian_traj_SystemCore_setup(robotics_slcore_internal_bl_j_T *obj)
17865{
17866 obj->isInitialized = 1;
17867 obj->TunablePropsChanged = false;
17868}
17869
17870// Model step function
17871void cartesian_trajectory_planner_2_step(void)
17872{
17873 // SignalConversion generated from: '<S13>/Coordinate Transformation Conversion' incorporates:
17874 // MATLABSystem: '<S13>/Get Parameter3'
17875 // MATLABSystem: '<S13>/Get Parameter4'
17876 // MATLABSystem: '<S13>/Get Parameter5'
17877 // MATLABSystem: '<S13>/Get Parameter6'
17878
17879 ParamGet_cartesian_trajectory_planner_2_290.get_parameter
17880 (&cartesian_trajectory_planner__B.TmpSignalConversionAtCoordi[0]);
17881 ParamGet_cartesian_trajectory_planner_2_291.get_parameter
17882 (&cartesian_trajectory_planner__B.TmpSignalConversionAtCoordi[1]);
17883 ParamGet_cartesian_trajectory_planner_2_292.get_parameter
17884 (&cartesian_trajectory_planner__B.TmpSignalConversionAtCoordi[2]);
17885 ParamGet_cartesian_trajectory_planner_2_293.get_parameter
17886 (&cartesian_trajectory_planner__B.TmpSignalConversionAtCoordi[3]);
17887
17888 // SignalConversion generated from: '<S13>/Coordinate Transformation Conversion' incorporates:
17889 // MATLABSystem: '<S13>/Get Parameter'
17890 // MATLABSystem: '<S13>/Get Parameter1'
17891 // MATLABSystem: '<S13>/Get Parameter2'
17892
17893 ParamGet_cartesian_trajectory_planner_2_287.get_parameter
17894 (&cartesian_trajectory_planner__B.TmpSignalConversionAtCoor_p[0]);
17895 ParamGet_cartesian_trajectory_planner_2_288.get_parameter
17896 (&cartesian_trajectory_planner__B.TmpSignalConversionAtCoor_p[1]);
17897 ParamGet_cartesian_trajectory_planner_2_289.get_parameter
17898 (&cartesian_trajectory_planner__B.TmpSignalConversionAtCoor_p[2]);
17899 CoordinateTransformationConvers
17900 (cartesian_trajectory_planner__B.TmpSignalConversionAtCoordi,
17901 cartesian_trajectory_planner__B.TmpSignalConversionAtCoor_p,
17902 &cartesian_trajectory_planner__B.CoordinateTransformationConv_pn);
17903
17904 // SignalConversion generated from: '<S12>/Coordinate Transformation Conversion' incorporates:
17905 // MATLABSystem: '<S12>/Get Parameter3'
17906 // MATLABSystem: '<S12>/Get Parameter4'
17907 // MATLABSystem: '<S12>/Get Parameter5'
17908 // MATLABSystem: '<S12>/Get Parameter6'
17909
17910 ParamGet_cartesian_trajectory_planner_2_278.get_parameter
17911 (&cartesian_trajectory_planner__B.TmpSignalConversionAtCoor_l[0]);
17912 ParamGet_cartesian_trajectory_planner_2_279.get_parameter
17913 (&cartesian_trajectory_planner__B.TmpSignalConversionAtCoor_l[1]);
17914 ParamGet_cartesian_trajectory_planner_2_280.get_parameter
17915 (&cartesian_trajectory_planner__B.TmpSignalConversionAtCoor_l[2]);
17916 ParamGet_cartesian_trajectory_planner_2_281.get_parameter
17917 (&cartesian_trajectory_planner__B.TmpSignalConversionAtCoor_l[3]);
17918
17919 // SignalConversion generated from: '<S12>/Coordinate Transformation Conversion' incorporates:
17920 // MATLABSystem: '<S12>/Get Parameter'
17921 // MATLABSystem: '<S12>/Get Parameter1'
17922 // MATLABSystem: '<S12>/Get Parameter2'
17923
17924 ParamGet_cartesian_trajectory_planner_2_275.get_parameter
17925 (&cartesian_trajectory_planner__B.TmpSignalConversionAtCoo_pn[0]);
17926 ParamGet_cartesian_trajectory_planner_2_276.get_parameter
17927 (&cartesian_trajectory_planner__B.TmpSignalConversionAtCoo_pn[1]);
17928 ParamGet_cartesian_trajectory_planner_2_277.get_parameter
17929 (&cartesian_trajectory_planner__B.TmpSignalConversionAtCoo_pn[2]);
17930 CoordinateTransformationConvers
17931 (cartesian_trajectory_planner__B.TmpSignalConversionAtCoor_l,
17932 cartesian_trajectory_planner__B.TmpSignalConversionAtCoo_pn,
17933 &cartesian_trajectory_planner__B.CoordinateTransformationConve_p);
17934
17935 // SignalConversion generated from: '<S2>/MATLAB System' incorporates:
17936 // MATLABSystem: '<S10>/Get Parameter'
17937 // MATLABSystem: '<S10>/Get Parameter1'
17938 // MATLABSystem: '<S10>/Get Parameter2'
17939 // MATLABSystem: '<S10>/Get Parameter3'
17940 // MATLABSystem: '<S10>/Get Parameter4'
17941 // MATLABSystem: '<S10>/Get Parameter5'
17942
17943 ParamGet_cartesian_trajectory_planner_2_299.get_parameter
17944 (&cartesian_trajectory_planner__B.TmpSignalConversionAtMATLAB[0]);
17945 ParamGet_cartesian_trajectory_planner_2_300.get_parameter
17946 (&cartesian_trajectory_planner__B.TmpSignalConversionAtMATLAB[1]);
17947 ParamGet_cartesian_trajectory_planner_2_301.get_parameter
17948 (&cartesian_trajectory_planner__B.TmpSignalConversionAtMATLAB[2]);
17949 ParamGet_cartesian_trajectory_planner_2_302.get_parameter
17950 (&cartesian_trajectory_planner__B.TmpSignalConversionAtMATLAB[3]);
17951 ParamGet_cartesian_trajectory_planner_2_303.get_parameter
17952 (&cartesian_trajectory_planner__B.TmpSignalConversionAtMATLAB[4]);
17953 ParamGet_cartesian_trajectory_planner_2_304.get_parameter
17954 (&cartesian_trajectory_planner__B.TmpSignalConversionAtMATLAB[5]);
17955
17956 // Constant: '<Root>/Constant'
17957 cartesian_trajecto_MATLABSystem
17958 (cartesian_trajectory_planner__B.CoordinateTransformationConve_p.CoordinateTransformationConve_g,
17959 cartesian_trajectory_planner__B.TmpSignalConversionAtMATLAB,
17960 cartesian_trajectory_planner__P.Constant_Value_o,
17961 &cartesian_trajectory_planner__B.MATLABSystem,
17962 &cartesian_trajectory_planner_DW.MATLABSystem);
17963
17964 // SignalConversion generated from: '<S2>/MATLAB System'
17965 for (cartesian_trajectory_planner__B.i = 0; cartesian_trajectory_planner__B.i <
17966 6; cartesian_trajectory_planner__B.i++) {
17967 cartesian_trajectory_planner__B.TmpMLSysMemLayoutBufferAt_m[cartesian_trajectory_planner__B.i]
17968 =
17969 cartesian_trajectory_planner__B.MATLABSystem.MATLABSystem_o1[cartesian_trajectory_planner__B.i];
17970 }
17971
17972 // End of SignalConversion generated from: '<S2>/MATLAB System'
17973 cartesian_trajecto_MATLABSystem
17974 (cartesian_trajectory_planner__B.CoordinateTransformationConv_pn.CoordinateTransformationConve_g,
17975 cartesian_trajectory_planner__B.TmpSignalConversionAtMATLAB,
17976 cartesian_trajectory_planner__B.TmpMLSysMemLayoutBufferAt_m,
17977 &cartesian_trajectory_planner__B.MATLABSystem_p,
17978 &cartesian_trajectory_planner_DW.MATLABSystem_p);
17979
17980 // SignalConversion generated from: '<S3>/MATLAB System'
17981 for (cartesian_trajectory_planner__B.i = 0; cartesian_trajectory_planner__B.i <
17982 6; cartesian_trajectory_planner__B.i++) {
17983 cartesian_trajectory_planner__B.MatrixConcatenate[cartesian_trajectory_planner__B.i]
17984 =
17985 cartesian_trajectory_planner__B.MATLABSystem_p.MATLABSystem_o1[cartesian_trajectory_planner__B.i];
17986 }
17987
17988 // End of SignalConversion generated from: '<S3>/MATLAB System'
17989
17990 // SignalConversion generated from: '<Root>/Matrix Concatenate'
17991 for (cartesian_trajectory_planner__B.i = 0; cartesian_trajectory_planner__B.i <
17992 6; cartesian_trajectory_planner__B.i++) {
17993 cartesian_trajectory_planner__B.MatrixConcatenate[cartesian_trajectory_planner__B.i
17994 + 6] =
17995 cartesian_trajectory_planner__B.TmpMLSysMemLayoutBufferAt_m[cartesian_trajectory_planner__B.i];
17996 }
17997
17998 // End of SignalConversion generated from: '<Root>/Matrix Concatenate'
17999
18000 // MATLABSystem: '<S9>/Get Parameter3'
18001 ParamGet_cartesian_trajectory_planner_2_272.get_parameter
18002 (&cartesian_trajectory_planner__B.value);
18003
18004 // MATLABSystem: '<S9>/Get Parameter2'
18005 ParamGet_cartesian_trajectory_planner_2_271.get_parameter
18006 (&cartesian_trajectory_planner__B.delayed_time);
18007
18008 // MATLAB Function: '<Root>/MATLAB Function' incorporates:
18009 // MATLABSystem: '<S9>/Get Parameter2'
18010 // MATLABSystem: '<S9>/Get Parameter3'
18011
18012 cartesian_trajectory_planner__B.t_up =
18013 cartesian_trajectory_planner__B.delayed_time /
18014 cartesian_trajectory_planner__B.value;
18015 for (cartesian_trajectory_planner__B.i = 0; cartesian_trajectory_planner__B.i <
18016 6; cartesian_trajectory_planner__B.i++) {
18017 cartesian_trajectory_planner__B.rtb_res_max_vel_d =
18018 cartesian_trajectory_planner__B.delayed_time;
18019 cartesian_trajectory_planner__B.d = fabs
18020 (cartesian_trajectory_planner__B.MatrixConcatenate[cartesian_trajectory_planner__B.i]
18021 - cartesian_trajectory_planner__B.MatrixConcatenate[cartesian_trajectory_planner__B.i
18022 + 6]);
18023 if (cartesian_trajectory_planner__B.d <
18024 cartesian_trajectory_planner__B.delayed_time *
18025 cartesian_trajectory_planner__B.t_up) {
18026 cartesian_trajectory_planner__B.rtb_res_max_vel_d =
18027 cartesian_trajectory_planner__B.d / cartesian_trajectory_planner__B.t_up;
18028 if (cartesian_trajectory_planner__B.rtb_res_max_vel_d == 0.0) {
18029 cartesian_trajectory_planner__B.rtb_res_max_vel_d = 0.01;
18030 }
18031 }
18032
18033 cartesian_trajectory_planner__B.res_max_vel[cartesian_trajectory_planner__B.i]
18034 = cartesian_trajectory_planner__B.rtb_res_max_vel_d;
18035 }
18036
18037 // End of MATLAB Function: '<Root>/MATLAB Function'
18038
18039 // Outputs for Atomic SubSystem: '<Root>/Subscribe'
18040 // MATLABSystem: '<S8>/SourceBlock' incorporates:
18041 // Inport: '<S11>/In1'
18042
18043 cartesian_trajectory_planner__B.b_varargout_1_h =
18044 Sub_cartesian_trajectory_planner_2_267.getLatestMessage
18045 (&cartesian_trajectory_planner__B.b_varargout_2_j);
18046
18047 // Outputs for Enabled SubSystem: '<S8>/Enabled Subsystem' incorporates:
18048 // EnablePort: '<S11>/Enable'
18049
18050 if (cartesian_trajectory_planner__B.b_varargout_1_h) {
18051 cartesian_trajectory_planner__B.In1 =
18052 cartesian_trajectory_planner__B.b_varargout_2_j;
18053 }
18054
18055 // End of MATLABSystem: '<S8>/SourceBlock'
18056 // End of Outputs for SubSystem: '<S8>/Enabled Subsystem'
18057 // End of Outputs for SubSystem: '<Root>/Subscribe'
18058
18059 // MATLABSystem: '<S9>/Get Parameter'
18060 ParamGet_cartesian_trajectory_planner_2_270.get_parameter
18061 (&cartesian_trajectory_planner__B.delayed_time);
18062
18063 // MATLAB Function: '<Root>/MATLAB Function1' incorporates:
18064 // MATLABSystem: '<S9>/Get Parameter'
18065
18066 cartesian_trajectory_planner__B.delayed_time =
18067 (cartesian_trajectory_planner__B.In1.Clock_.Nsec / 1.0E+9 +
18068 cartesian_trajectory_planner__B.In1.Clock_.Sec) -
18069 cartesian_trajectory_planner__B.delayed_time;
18070 if (cartesian_trajectory_planner__B.delayed_time < 0.0) {
18071 cartesian_trajectory_planner__B.delayed_time = 0.0;
18072 }
18073
18074 // End of MATLAB Function: '<Root>/MATLAB Function1'
18075
18076 // MATLABSystem: '<Root>/Trapezoidal Velocity Profile Trajectory' incorporates:
18077 // MATLABSystem: '<S9>/Get Parameter3'
18078
18079 if (cartesian_trajectory_planner_DW.obj.TunablePropsChanged) {
18080 cartesian_trajectory_planner_DW.obj.TunablePropsChanged = false;
18081 }
18082
18083 cartesi_TrapVelTrajSys_stepImpl(cartesian_trajectory_planner__B.delayed_time,
18084 cartesian_trajectory_planner__B.MatrixConcatenate,
18085 cartesian_trajectory_planner__B.res_max_vel,
18086 cartesian_trajectory_planner__B.value,
18087 cartesian_trajectory_planner__B.b_varargout_1,
18088 cartesian_trajectory_planner__B.b_varargout_2,
18089 cartesian_trajectory_planner__B.b_varargout_3);
18090
18091 // MATLAB Function: '<Root>/MATLAB Function2' incorporates:
18092 // Constant: '<S1>/Constant'
18093 // MATLABSystem: '<Root>/Trapezoidal Velocity Profile Trajectory'
18094
18095 cartesian_trajectory_planner__B.msg =
18096 cartesian_trajectory_planner__P.Constant_Value;
18097 cartesian_trajectory_planner__B.msg.Velocities_SL_Info.CurrentLength = 6U;
18098 cartesian_trajectory_planner__B.msg.Positions_SL_Info.CurrentLength = 6U;
18099 cartesian_trajectory_planner__B.msg.Accelerations_SL_Info.CurrentLength = 6U;
18100 for (cartesian_trajectory_planner__B.i = 0; cartesian_trajectory_planner__B.i <
18101 6; cartesian_trajectory_planner__B.i++) {
18102 cartesian_trajectory_planner__B.msg.Velocities[cartesian_trajectory_planner__B.i]
18103 =
18104 cartesian_trajectory_planner__B.b_varargout_2[cartesian_trajectory_planner__B.i];
18105 cartesian_trajectory_planner__B.msg.Positions[cartesian_trajectory_planner__B.i]
18106 =
18107 cartesian_trajectory_planner__B.b_varargout_1[cartesian_trajectory_planner__B.i];
18108 cartesian_trajectory_planner__B.msg.Accelerations[cartesian_trajectory_planner__B.i]
18109 =
18110 cartesian_trajectory_planner__B.b_varargout_3[cartesian_trajectory_planner__B.i];
18111 }
18112
18113 // End of MATLAB Function: '<Root>/MATLAB Function2'
18114
18115 // Outputs for Atomic SubSystem: '<Root>/Publish'
18116 // MATLABSystem: '<S7>/SinkBlock'
18117 Pub_cartesian_trajectory_planner_2_266.publish
18118 (&cartesian_trajectory_planner__B.msg);
18119
18120 // End of Outputs for SubSystem: '<Root>/Publish'
18121}
18122
18123// Model initialize function
18124void cartesian_trajectory_planner_2_initialize(void)
18125{
18126 // Registration code
18127
18128 // initialize non-finites
18129 rt_InitInfAndNaN(sizeof(real_T));
18130
18131 {
18132 static const char_T tmp[6] = { '/', 'c', 'l', 'o', 'c', 'k' };
18133
18134 static const char_T tmp_0[17] = { '/', 'j', 'o', 'i', 'n', 't', '_', 't',
18135 'r', 'a', 'j', 'e', 'c', 't', 'o', 'r', 'y' };
18136
18137 static const char_T tmp_1[5] = { '/', 'o', 'f', '_', 'x' };
18138
18139 static const char_T tmp_2[5] = { '/', 'o', 'f', '_', 'y' };
18140
18141 static const char_T tmp_3[5] = { '/', 'o', 'f', '_', 'z' };
18142
18143 static const char_T tmp_4[5] = { '/', 'o', 'f', '_', 'w' };
18144
18145 static const char_T tmp_5[5] = { '/', 'p', 'f', '_', 'x' };
18146
18147 static const char_T tmp_6[5] = { '/', 'p', 'f', '_', 'y' };
18148
18149 static const char_T tmp_7[5] = { '/', 'p', 'f', '_', 'z' };
18150
18151 static const char_T tmp_8[5] = { '/', 'o', 'i', '_', 'x' };
18152
18153 static const char_T tmp_9[5] = { '/', 'o', 'i', '_', 'y' };
18154
18155 static const char_T tmp_a[5] = { '/', 'o', 'i', '_', 'z' };
18156
18157 static const char_T tmp_b[5] = { '/', 'o', 'i', '_', 'w' };
18158
18159 static const char_T tmp_c[5] = { '/', 'p', 'i', '_', 'x' };
18160
18161 static const char_T tmp_d[5] = { '/', 'p', 'i', '_', 'y' };
18162
18163 static const char_T tmp_e[5] = { '/', 'p', 'i', '_', 'z' };
18164
18165 static const char_T tmp_f[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
18166 'o', 'x' };
18167
18168 static const char_T tmp_g[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
18169 'o', 'y' };
18170
18171 static const char_T tmp_h[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
18172 'o', 'z' };
18173
18174 static const char_T tmp_i[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
18175 'p', 'x' };
18176
18177 static const char_T tmp_j[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
18178 'p', 'y' };
18179
18180 static const char_T tmp_k[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
18181 'p', 'z' };
18182
18183 static const char_T tmp_l[12] = { '/', 'm', 'a', 'x', '_', 'a', 'n', 'g',
18184 '_', 'a', 'c', 'c' };
18185
18186 static const char_T tmp_m[12] = { '/', 'm', 'a', 'x', '_', 'a', 'n', 'g',
18187 '_', 'v', 'e', 'l' };
18188
18189 static const char_T tmp_n[12] = { '/', 's', 't', 'a', 'r', 't', '_', 'd',
18190 'e', 'l', 'a', 'y' };
18191
18192 // SystemInitialize for Atomic SubSystem: '<Root>/Subscribe'
18193 // SystemInitialize for Enabled SubSystem: '<S8>/Enabled Subsystem'
18194 // SystemInitialize for Outport: '<S11>/Out1'
18195 cartesian_trajectory_planner__B.In1 =
18196 cartesian_trajectory_planner__P.Out1_Y0;
18197
18198 // End of SystemInitialize for SubSystem: '<S8>/Enabled Subsystem'
18199
18200 // Start for MATLABSystem: '<S8>/SourceBlock'
18201 cartesian_trajectory_planner_DW.obj_cs.matlabCodegenIsDeleted = true;
18202 cartesian_trajectory_planner_DW.obj_cs.isInitialized = 0;
18203 cartesian_trajectory_planner_DW.obj_cs.matlabCodegenIsDeleted = false;
18204 cartesian_trajectory_planner_DW.obj_cs.isSetupComplete = false;
18205 cartesian_trajectory_planner_DW.obj_cs.isInitialized = 1;
18206 for (cartesian_trajectory_planner__B.i1 = 0;
18207 cartesian_trajectory_planner__B.i1 < 6;
18208 cartesian_trajectory_planner__B.i1++) {
18209 cartesian_trajectory_planner__B.cv3[cartesian_trajectory_planner__B.i1] =
18210 tmp[cartesian_trajectory_planner__B.i1];
18211 }
18212
18213 cartesian_trajectory_planner__B.cv3[6] = '\x00';
18214 Sub_cartesian_trajectory_planner_2_267.createSubscriber
18215 (cartesian_trajectory_planner__B.cv3, 1);
18216 cartesian_trajectory_planner_DW.obj_cs.isSetupComplete = true;
18217
18218 // End of Start for MATLABSystem: '<S8>/SourceBlock'
18219 // End of SystemInitialize for SubSystem: '<Root>/Subscribe'
18220
18221 // SystemInitialize for Atomic SubSystem: '<Root>/Publish'
18222 // Start for MATLABSystem: '<S7>/SinkBlock'
18223 cartesian_trajectory_planner_DW.obj_ej.matlabCodegenIsDeleted = true;
18224 cartesian_trajectory_planner_DW.obj_ej.isInitialized = 0;
18225 cartesian_trajectory_planner_DW.obj_ej.matlabCodegenIsDeleted = false;
18226 cartesian_trajectory_planner_DW.obj_ej.isSetupComplete = false;
18227 cartesian_trajectory_planner_DW.obj_ej.isInitialized = 1;
18228 for (cartesian_trajectory_planner__B.i1 = 0;
18229 cartesian_trajectory_planner__B.i1 < 17;
18230 cartesian_trajectory_planner__B.i1++) {
18231 cartesian_trajectory_planner__B.cv[cartesian_trajectory_planner__B.i1] =
18232 tmp_0[cartesian_trajectory_planner__B.i1];
18233 }
18234
18235 cartesian_trajectory_planner__B.cv[17] = '\x00';
18236 Pub_cartesian_trajectory_planner_2_266.createPublisher
18237 (cartesian_trajectory_planner__B.cv, 1);
18238 cartesian_trajectory_planner_DW.obj_ej.isSetupComplete = true;
18239
18240 // End of Start for MATLABSystem: '<S7>/SinkBlock'
18241 // End of SystemInitialize for SubSystem: '<Root>/Publish'
18242
18243 // Start for MATLABSystem: '<S13>/Get Parameter3'
18244 cartesian_trajectory_planner_DW.obj_o.matlabCodegenIsDeleted = true;
18245 cartesian_trajectory_planner_DW.obj_o.isInitialized = 0;
18246 cartesian_trajectory_planner_DW.obj_o.matlabCodegenIsDeleted = false;
18247 cartesian_trajectory_planner_DW.obj_o.isSetupComplete = false;
18248 cartesian_trajectory_planner_DW.obj_o.isInitialized = 1;
18249 for (cartesian_trajectory_planner__B.i1 = 0;
18250 cartesian_trajectory_planner__B.i1 < 5;
18251 cartesian_trajectory_planner__B.i1++) {
18252 cartesian_trajectory_planner__B.cv4[cartesian_trajectory_planner__B.i1] =
18253 tmp_1[cartesian_trajectory_planner__B.i1];
18254 }
18255
18256 cartesian_trajectory_planner__B.cv4[5] = '\x00';
18257 ParamGet_cartesian_trajectory_planner_2_290.initialize
18258 (cartesian_trajectory_planner__B.cv4);
18259 ParamGet_cartesian_trajectory_planner_2_290.initialize_error_codes(0, 1, 2,
18260 3);
18261 ParamGet_cartesian_trajectory_planner_2_290.set_initial_value(0.39);
18262 cartesian_trajectory_planner_DW.obj_o.isSetupComplete = true;
18263
18264 // End of Start for MATLABSystem: '<S13>/Get Parameter3'
18265
18266 // Start for MATLABSystem: '<S13>/Get Parameter4'
18267 cartesian_trajectory_planner_DW.obj_es.matlabCodegenIsDeleted = true;
18268 cartesian_trajectory_planner_DW.obj_es.isInitialized = 0;
18269 cartesian_trajectory_planner_DW.obj_es.matlabCodegenIsDeleted = false;
18270 cartesian_trajectory_planner_DW.obj_es.isSetupComplete = false;
18271 cartesian_trajectory_planner_DW.obj_es.isInitialized = 1;
18272 for (cartesian_trajectory_planner__B.i1 = 0;
18273 cartesian_trajectory_planner__B.i1 < 5;
18274 cartesian_trajectory_planner__B.i1++) {
18275 cartesian_trajectory_planner__B.cv4[cartesian_trajectory_planner__B.i1] =
18276 tmp_2[cartesian_trajectory_planner__B.i1];
18277 }
18278
18279 cartesian_trajectory_planner__B.cv4[5] = '\x00';
18280 ParamGet_cartesian_trajectory_planner_2_291.initialize
18281 (cartesian_trajectory_planner__B.cv4);
18282 ParamGet_cartesian_trajectory_planner_2_291.initialize_error_codes(0, 1, 2,
18283 3);
18284 ParamGet_cartesian_trajectory_planner_2_291.set_initial_value(0.89);
18285 cartesian_trajectory_planner_DW.obj_es.isSetupComplete = true;
18286
18287 // End of Start for MATLABSystem: '<S13>/Get Parameter4'
18288
18289 // Start for MATLABSystem: '<S13>/Get Parameter5'
18290 cartesian_trajectory_planner_DW.obj_f4.matlabCodegenIsDeleted = true;
18291 cartesian_trajectory_planner_DW.obj_f4.isInitialized = 0;
18292 cartesian_trajectory_planner_DW.obj_f4.matlabCodegenIsDeleted = false;
18293 cartesian_trajectory_planner_DW.obj_f4.isSetupComplete = false;
18294 cartesian_trajectory_planner_DW.obj_f4.isInitialized = 1;
18295 for (cartesian_trajectory_planner__B.i1 = 0;
18296 cartesian_trajectory_planner__B.i1 < 5;
18297 cartesian_trajectory_planner__B.i1++) {
18298 cartesian_trajectory_planner__B.cv4[cartesian_trajectory_planner__B.i1] =
18299 tmp_3[cartesian_trajectory_planner__B.i1];
18300 }
18301
18302 cartesian_trajectory_planner__B.cv4[5] = '\x00';
18303 ParamGet_cartesian_trajectory_planner_2_292.initialize
18304 (cartesian_trajectory_planner__B.cv4);
18305 ParamGet_cartesian_trajectory_planner_2_292.initialize_error_codes(0, 1, 2,
18306 3);
18307 ParamGet_cartesian_trajectory_planner_2_292.set_initial_value(0.2);
18308 cartesian_trajectory_planner_DW.obj_f4.isSetupComplete = true;
18309
18310 // End of Start for MATLABSystem: '<S13>/Get Parameter5'
18311
18312 // Start for MATLABSystem: '<S13>/Get Parameter6'
18313 cartesian_trajectory_planner_DW.obj_en.matlabCodegenIsDeleted = true;
18314 cartesian_trajectory_planner_DW.obj_en.isInitialized = 0;
18315 cartesian_trajectory_planner_DW.obj_en.matlabCodegenIsDeleted = false;
18316 cartesian_trajectory_planner_DW.obj_en.isSetupComplete = false;
18317 cartesian_trajectory_planner_DW.obj_en.isInitialized = 1;
18318 for (cartesian_trajectory_planner__B.i1 = 0;
18319 cartesian_trajectory_planner__B.i1 < 5;
18320 cartesian_trajectory_planner__B.i1++) {
18321 cartesian_trajectory_planner__B.cv4[cartesian_trajectory_planner__B.i1] =
18322 tmp_4[cartesian_trajectory_planner__B.i1];
18323 }
18324
18325 cartesian_trajectory_planner__B.cv4[5] = '\x00';
18326 ParamGet_cartesian_trajectory_planner_2_293.initialize
18327 (cartesian_trajectory_planner__B.cv4);
18328 ParamGet_cartesian_trajectory_planner_2_293.initialize_error_codes(0, 1, 2,
18329 3);
18330 ParamGet_cartesian_trajectory_planner_2_293.set_initial_value(-0.12);
18331 cartesian_trajectory_planner_DW.obj_en.isSetupComplete = true;
18332
18333 // End of Start for MATLABSystem: '<S13>/Get Parameter6'
18334
18335 // Start for MATLABSystem: '<S13>/Get Parameter'
18336 cartesian_trajectory_planner_DW.obj_b.matlabCodegenIsDeleted = true;
18337 cartesian_trajectory_planner_DW.obj_b.isInitialized = 0;
18338 cartesian_trajectory_planner_DW.obj_b.matlabCodegenIsDeleted = false;
18339 cartesian_trajectory_planner_DW.obj_b.isSetupComplete = false;
18340 cartesian_trajectory_planner_DW.obj_b.isInitialized = 1;
18341 for (cartesian_trajectory_planner__B.i1 = 0;
18342 cartesian_trajectory_planner__B.i1 < 5;
18343 cartesian_trajectory_planner__B.i1++) {
18344 cartesian_trajectory_planner__B.cv4[cartesian_trajectory_planner__B.i1] =
18345 tmp_5[cartesian_trajectory_planner__B.i1];
18346 }
18347
18348 cartesian_trajectory_planner__B.cv4[5] = '\x00';
18349 ParamGet_cartesian_trajectory_planner_2_287.initialize
18350 (cartesian_trajectory_planner__B.cv4);
18351 ParamGet_cartesian_trajectory_planner_2_287.initialize_error_codes(0, 1, 2,
18352 3);
18353 ParamGet_cartesian_trajectory_planner_2_287.set_initial_value(0.21);
18354 cartesian_trajectory_planner_DW.obj_b.isSetupComplete = true;
18355
18356 // End of Start for MATLABSystem: '<S13>/Get Parameter'
18357
18358 // Start for MATLABSystem: '<S13>/Get Parameter1'
18359 cartesian_trajectory_planner_DW.obj_i.matlabCodegenIsDeleted = true;
18360 cartesian_trajectory_planner_DW.obj_i.isInitialized = 0;
18361 cartesian_trajectory_planner_DW.obj_i.matlabCodegenIsDeleted = false;
18362 cartesian_trajectory_planner_DW.obj_i.isSetupComplete = false;
18363 cartesian_trajectory_planner_DW.obj_i.isInitialized = 1;
18364 for (cartesian_trajectory_planner__B.i1 = 0;
18365 cartesian_trajectory_planner__B.i1 < 5;
18366 cartesian_trajectory_planner__B.i1++) {
18367 cartesian_trajectory_planner__B.cv4[cartesian_trajectory_planner__B.i1] =
18368 tmp_6[cartesian_trajectory_planner__B.i1];
18369 }
18370
18371 cartesian_trajectory_planner__B.cv4[5] = '\x00';
18372 ParamGet_cartesian_trajectory_planner_2_288.initialize
18373 (cartesian_trajectory_planner__B.cv4);
18374 ParamGet_cartesian_trajectory_planner_2_288.initialize_error_codes(0, 1, 2,
18375 3);
18376 ParamGet_cartesian_trajectory_planner_2_288.set_initial_value(0.89);
18377 cartesian_trajectory_planner_DW.obj_i.isSetupComplete = true;
18378
18379 // End of Start for MATLABSystem: '<S13>/Get Parameter1'
18380
18381 // Start for MATLABSystem: '<S13>/Get Parameter2'
18382 cartesian_trajectory_planner_DW.obj_db.matlabCodegenIsDeleted = true;
18383 cartesian_trajectory_planner_DW.obj_db.isInitialized = 0;
18384 cartesian_trajectory_planner_DW.obj_db.matlabCodegenIsDeleted = false;
18385 cartesian_trajectory_planner_DW.obj_db.isSetupComplete = false;
18386 cartesian_trajectory_planner_DW.obj_db.isInitialized = 1;
18387 for (cartesian_trajectory_planner__B.i1 = 0;
18388 cartesian_trajectory_planner__B.i1 < 5;
18389 cartesian_trajectory_planner__B.i1++) {
18390 cartesian_trajectory_planner__B.cv4[cartesian_trajectory_planner__B.i1] =
18391 tmp_7[cartesian_trajectory_planner__B.i1];
18392 }
18393
18394 cartesian_trajectory_planner__B.cv4[5] = '\x00';
18395 ParamGet_cartesian_trajectory_planner_2_289.initialize
18396 (cartesian_trajectory_planner__B.cv4);
18397 ParamGet_cartesian_trajectory_planner_2_289.initialize_error_codes(0, 1, 2,
18398 3);
18399 ParamGet_cartesian_trajectory_planner_2_289.set_initial_value(0.2);
18400 cartesian_trajectory_planner_DW.obj_db.isSetupComplete = true;
18401
18402 // End of Start for MATLABSystem: '<S13>/Get Parameter2'
18403 CoordinateTransformationCo_Init
18404 (&cartesian_trajectory_planner_DW.CoordinateTransformationConv_pn);
18405
18406 // Start for MATLABSystem: '<S12>/Get Parameter3'
18407 cartesian_trajectory_planner_DW.obj_e0.matlabCodegenIsDeleted = true;
18408 cartesian_trajectory_planner_DW.obj_e0.isInitialized = 0;
18409 cartesian_trajectory_planner_DW.obj_e0.matlabCodegenIsDeleted = false;
18410 cartesian_trajectory_planner_DW.obj_e0.isSetupComplete = false;
18411 cartesian_trajectory_planner_DW.obj_e0.isInitialized = 1;
18412 for (cartesian_trajectory_planner__B.i1 = 0;
18413 cartesian_trajectory_planner__B.i1 < 5;
18414 cartesian_trajectory_planner__B.i1++) {
18415 cartesian_trajectory_planner__B.cv4[cartesian_trajectory_planner__B.i1] =
18416 tmp_8[cartesian_trajectory_planner__B.i1];
18417 }
18418
18419 cartesian_trajectory_planner__B.cv4[5] = '\x00';
18420 ParamGet_cartesian_trajectory_planner_2_278.initialize
18421 (cartesian_trajectory_planner__B.cv4);
18422 ParamGet_cartesian_trajectory_planner_2_278.initialize_error_codes(0, 1, 2,
18423 3);
18424 ParamGet_cartesian_trajectory_planner_2_278.set_initial_value(0.0);
18425 cartesian_trajectory_planner_DW.obj_e0.isSetupComplete = true;
18426
18427 // End of Start for MATLABSystem: '<S12>/Get Parameter3'
18428
18429 // Start for MATLABSystem: '<S12>/Get Parameter4'
18430 cartesian_trajectory_planner_DW.obj_p.matlabCodegenIsDeleted = true;
18431 cartesian_trajectory_planner_DW.obj_p.isInitialized = 0;
18432 cartesian_trajectory_planner_DW.obj_p.matlabCodegenIsDeleted = false;
18433 cartesian_trajectory_planner_DW.obj_p.isSetupComplete = false;
18434 cartesian_trajectory_planner_DW.obj_p.isInitialized = 1;
18435 for (cartesian_trajectory_planner__B.i1 = 0;
18436 cartesian_trajectory_planner__B.i1 < 5;
18437 cartesian_trajectory_planner__B.i1++) {
18438 cartesian_trajectory_planner__B.cv4[cartesian_trajectory_planner__B.i1] =
18439 tmp_9[cartesian_trajectory_planner__B.i1];
18440 }
18441
18442 cartesian_trajectory_planner__B.cv4[5] = '\x00';
18443 ParamGet_cartesian_trajectory_planner_2_279.initialize
18444 (cartesian_trajectory_planner__B.cv4);
18445 ParamGet_cartesian_trajectory_planner_2_279.initialize_error_codes(0, 1, 2,
18446 3);
18447 ParamGet_cartesian_trajectory_planner_2_279.set_initial_value(0.0);
18448 cartesian_trajectory_planner_DW.obj_p.isSetupComplete = true;
18449
18450 // End of Start for MATLABSystem: '<S12>/Get Parameter4'
18451
18452 // Start for MATLABSystem: '<S12>/Get Parameter5'
18453 cartesian_trajectory_planner_DW.obj_g.matlabCodegenIsDeleted = true;
18454 cartesian_trajectory_planner_DW.obj_g.isInitialized = 0;
18455 cartesian_trajectory_planner_DW.obj_g.matlabCodegenIsDeleted = false;
18456 cartesian_trajectory_planner_DW.obj_g.isSetupComplete = false;
18457 cartesian_trajectory_planner_DW.obj_g.isInitialized = 1;
18458 for (cartesian_trajectory_planner__B.i1 = 0;
18459 cartesian_trajectory_planner__B.i1 < 5;
18460 cartesian_trajectory_planner__B.i1++) {
18461 cartesian_trajectory_planner__B.cv4[cartesian_trajectory_planner__B.i1] =
18462 tmp_a[cartesian_trajectory_planner__B.i1];
18463 }
18464
18465 cartesian_trajectory_planner__B.cv4[5] = '\x00';
18466 ParamGet_cartesian_trajectory_planner_2_280.initialize
18467 (cartesian_trajectory_planner__B.cv4);
18468 ParamGet_cartesian_trajectory_planner_2_280.initialize_error_codes(0, 1, 2,
18469 3);
18470 ParamGet_cartesian_trajectory_planner_2_280.set_initial_value(0.0);
18471 cartesian_trajectory_planner_DW.obj_g.isSetupComplete = true;
18472
18473 // End of Start for MATLABSystem: '<S12>/Get Parameter5'
18474
18475 // Start for MATLABSystem: '<S12>/Get Parameter6'
18476 cartesian_trajectory_planner_DW.obj_or.matlabCodegenIsDeleted = true;
18477 cartesian_trajectory_planner_DW.obj_or.isInitialized = 0;
18478 cartesian_trajectory_planner_DW.obj_or.matlabCodegenIsDeleted = false;
18479 cartesian_trajectory_planner_DW.obj_or.isSetupComplete = false;
18480 cartesian_trajectory_planner_DW.obj_or.isInitialized = 1;
18481 for (cartesian_trajectory_planner__B.i1 = 0;
18482 cartesian_trajectory_planner__B.i1 < 5;
18483 cartesian_trajectory_planner__B.i1++) {
18484 cartesian_trajectory_planner__B.cv4[cartesian_trajectory_planner__B.i1] =
18485 tmp_b[cartesian_trajectory_planner__B.i1];
18486 }
18487
18488 cartesian_trajectory_planner__B.cv4[5] = '\x00';
18489 ParamGet_cartesian_trajectory_planner_2_281.initialize
18490 (cartesian_trajectory_planner__B.cv4);
18491 ParamGet_cartesian_trajectory_planner_2_281.initialize_error_codes(0, 1, 2,
18492 3);
18493 ParamGet_cartesian_trajectory_planner_2_281.set_initial_value(1.0);
18494 cartesian_trajectory_planner_DW.obj_or.isSetupComplete = true;
18495
18496 // End of Start for MATLABSystem: '<S12>/Get Parameter6'
18497
18498 // Start for MATLABSystem: '<S12>/Get Parameter'
18499 cartesian_trajectory_planner_DW.obj_a3.matlabCodegenIsDeleted = true;
18500 cartesian_trajectory_planner_DW.obj_a3.isInitialized = 0;
18501 cartesian_trajectory_planner_DW.obj_a3.matlabCodegenIsDeleted = false;
18502 cartesian_trajectory_planner_DW.obj_a3.isSetupComplete = false;
18503 cartesian_trajectory_planner_DW.obj_a3.isInitialized = 1;
18504 for (cartesian_trajectory_planner__B.i1 = 0;
18505 cartesian_trajectory_planner__B.i1 < 5;
18506 cartesian_trajectory_planner__B.i1++) {
18507 cartesian_trajectory_planner__B.cv4[cartesian_trajectory_planner__B.i1] =
18508 tmp_c[cartesian_trajectory_planner__B.i1];
18509 }
18510
18511 cartesian_trajectory_planner__B.cv4[5] = '\x00';
18512 ParamGet_cartesian_trajectory_planner_2_275.initialize
18513 (cartesian_trajectory_planner__B.cv4);
18514 ParamGet_cartesian_trajectory_planner_2_275.initialize_error_codes(0, 1, 2,
18515 3);
18516 ParamGet_cartesian_trajectory_planner_2_275.set_initial_value(0.0);
18517 cartesian_trajectory_planner_DW.obj_a3.isSetupComplete = true;
18518
18519 // End of Start for MATLABSystem: '<S12>/Get Parameter'
18520
18521 // Start for MATLABSystem: '<S12>/Get Parameter1'
18522 cartesian_trajectory_planner_DW.obj_bh.matlabCodegenIsDeleted = true;
18523 cartesian_trajectory_planner_DW.obj_bh.isInitialized = 0;
18524 cartesian_trajectory_planner_DW.obj_bh.matlabCodegenIsDeleted = false;
18525 cartesian_trajectory_planner_DW.obj_bh.isSetupComplete = false;
18526 cartesian_trajectory_planner_DW.obj_bh.isInitialized = 1;
18527 for (cartesian_trajectory_planner__B.i1 = 0;
18528 cartesian_trajectory_planner__B.i1 < 5;
18529 cartesian_trajectory_planner__B.i1++) {
18530 cartesian_trajectory_planner__B.cv4[cartesian_trajectory_planner__B.i1] =
18531 tmp_d[cartesian_trajectory_planner__B.i1];
18532 }
18533
18534 cartesian_trajectory_planner__B.cv4[5] = '\x00';
18535 ParamGet_cartesian_trajectory_planner_2_276.initialize
18536 (cartesian_trajectory_planner__B.cv4);
18537 ParamGet_cartesian_trajectory_planner_2_276.initialize_error_codes(0, 1, 2,
18538 3);
18539 ParamGet_cartesian_trajectory_planner_2_276.set_initial_value(0.0);
18540 cartesian_trajectory_planner_DW.obj_bh.isSetupComplete = true;
18541
18542 // End of Start for MATLABSystem: '<S12>/Get Parameter1'
18543
18544 // Start for MATLABSystem: '<S12>/Get Parameter2'
18545 cartesian_trajectory_planner_DW.obj_b0.matlabCodegenIsDeleted = true;
18546 cartesian_trajectory_planner_DW.obj_b0.isInitialized = 0;
18547 cartesian_trajectory_planner_DW.obj_b0.matlabCodegenIsDeleted = false;
18548 cartesian_trajectory_planner_DW.obj_b0.isSetupComplete = false;
18549 cartesian_trajectory_planner_DW.obj_b0.isInitialized = 1;
18550 for (cartesian_trajectory_planner__B.i1 = 0;
18551 cartesian_trajectory_planner__B.i1 < 5;
18552 cartesian_trajectory_planner__B.i1++) {
18553 cartesian_trajectory_planner__B.cv4[cartesian_trajectory_planner__B.i1] =
18554 tmp_e[cartesian_trajectory_planner__B.i1];
18555 }
18556
18557 cartesian_trajectory_planner__B.cv4[5] = '\x00';
18558 ParamGet_cartesian_trajectory_planner_2_277.initialize
18559 (cartesian_trajectory_planner__B.cv4);
18560 ParamGet_cartesian_trajectory_planner_2_277.initialize_error_codes(0, 1, 2,
18561 3);
18562 ParamGet_cartesian_trajectory_planner_2_277.set_initial_value(0.99);
18563 cartesian_trajectory_planner_DW.obj_b0.isSetupComplete = true;
18564
18565 // End of Start for MATLABSystem: '<S12>/Get Parameter2'
18566 CoordinateTransformationCo_Init
18567 (&cartesian_trajectory_planner_DW.CoordinateTransformationConve_p);
18568
18569 // Start for MATLABSystem: '<S10>/Get Parameter'
18570 cartesian_trajectory_planner_DW.obj_d.matlabCodegenIsDeleted = true;
18571 cartesian_trajectory_planner_DW.obj_d.isInitialized = 0;
18572 cartesian_trajectory_planner_DW.obj_d.matlabCodegenIsDeleted = false;
18573 cartesian_trajectory_planner_DW.obj_d.isSetupComplete = false;
18574 cartesian_trajectory_planner_DW.obj_d.isInitialized = 1;
18575 for (cartesian_trajectory_planner__B.i1 = 0;
18576 cartesian_trajectory_planner__B.i1 < 10;
18577 cartesian_trajectory_planner__B.i1++) {
18578 cartesian_trajectory_planner__B.cv2[cartesian_trajectory_planner__B.i1] =
18579 tmp_f[cartesian_trajectory_planner__B.i1];
18580 }
18581
18582 cartesian_trajectory_planner__B.cv2[10] = '\x00';
18583 ParamGet_cartesian_trajectory_planner_2_299.initialize
18584 (cartesian_trajectory_planner__B.cv2);
18585 ParamGet_cartesian_trajectory_planner_2_299.initialize_error_codes(0, 1, 2,
18586 3);
18587 ParamGet_cartesian_trajectory_planner_2_299.set_initial_value(0.0);
18588 cartesian_trajectory_planner_DW.obj_d.isSetupComplete = true;
18589
18590 // End of Start for MATLABSystem: '<S10>/Get Parameter'
18591
18592 // Start for MATLABSystem: '<S10>/Get Parameter1'
18593 cartesian_trajectory_planner_DW.obj_m.matlabCodegenIsDeleted = true;
18594 cartesian_trajectory_planner_DW.obj_m.isInitialized = 0;
18595 cartesian_trajectory_planner_DW.obj_m.matlabCodegenIsDeleted = false;
18596 cartesian_trajectory_planner_DW.obj_m.isSetupComplete = false;
18597 cartesian_trajectory_planner_DW.obj_m.isInitialized = 1;
18598 for (cartesian_trajectory_planner__B.i1 = 0;
18599 cartesian_trajectory_planner__B.i1 < 10;
18600 cartesian_trajectory_planner__B.i1++) {
18601 cartesian_trajectory_planner__B.cv2[cartesian_trajectory_planner__B.i1] =
18602 tmp_g[cartesian_trajectory_planner__B.i1];
18603 }
18604
18605 cartesian_trajectory_planner__B.cv2[10] = '\x00';
18606 ParamGet_cartesian_trajectory_planner_2_300.initialize
18607 (cartesian_trajectory_planner__B.cv2);
18608 ParamGet_cartesian_trajectory_planner_2_300.initialize_error_codes(0, 1, 2,
18609 3);
18610 ParamGet_cartesian_trajectory_planner_2_300.set_initial_value(0.0);
18611 cartesian_trajectory_planner_DW.obj_m.isSetupComplete = true;
18612
18613 // End of Start for MATLABSystem: '<S10>/Get Parameter1'
18614
18615 // Start for MATLABSystem: '<S10>/Get Parameter2'
18616 cartesian_trajectory_planner_DW.obj_c.matlabCodegenIsDeleted = true;
18617 cartesian_trajectory_planner_DW.obj_c.isInitialized = 0;
18618 cartesian_trajectory_planner_DW.obj_c.matlabCodegenIsDeleted = false;
18619 cartesian_trajectory_planner_DW.obj_c.isSetupComplete = false;
18620 cartesian_trajectory_planner_DW.obj_c.isInitialized = 1;
18621 for (cartesian_trajectory_planner__B.i1 = 0;
18622 cartesian_trajectory_planner__B.i1 < 10;
18623 cartesian_trajectory_planner__B.i1++) {
18624 cartesian_trajectory_planner__B.cv2[cartesian_trajectory_planner__B.i1] =
18625 tmp_h[cartesian_trajectory_planner__B.i1];
18626 }
18627
18628 cartesian_trajectory_planner__B.cv2[10] = '\x00';
18629 ParamGet_cartesian_trajectory_planner_2_301.initialize
18630 (cartesian_trajectory_planner__B.cv2);
18631 ParamGet_cartesian_trajectory_planner_2_301.initialize_error_codes(0, 1, 2,
18632 3);
18633 ParamGet_cartesian_trajectory_planner_2_301.set_initial_value(0.99);
18634 cartesian_trajectory_planner_DW.obj_c.isSetupComplete = true;
18635
18636 // End of Start for MATLABSystem: '<S10>/Get Parameter2'
18637
18638 // Start for MATLABSystem: '<S10>/Get Parameter3'
18639 cartesian_trajectory_planner_DW.obj_f.matlabCodegenIsDeleted = true;
18640 cartesian_trajectory_planner_DW.obj_f.isInitialized = 0;
18641 cartesian_trajectory_planner_DW.obj_f.matlabCodegenIsDeleted = false;
18642 cartesian_trajectory_planner_DW.obj_f.isSetupComplete = false;
18643 cartesian_trajectory_planner_DW.obj_f.isInitialized = 1;
18644 for (cartesian_trajectory_planner__B.i1 = 0;
18645 cartesian_trajectory_planner__B.i1 < 10;
18646 cartesian_trajectory_planner__B.i1++) {
18647 cartesian_trajectory_planner__B.cv2[cartesian_trajectory_planner__B.i1] =
18648 tmp_i[cartesian_trajectory_planner__B.i1];
18649 }
18650
18651 cartesian_trajectory_planner__B.cv2[10] = '\x00';
18652 ParamGet_cartesian_trajectory_planner_2_302.initialize
18653 (cartesian_trajectory_planner__B.cv2);
18654 ParamGet_cartesian_trajectory_planner_2_302.initialize_error_codes(0, 1, 2,
18655 3);
18656 ParamGet_cartesian_trajectory_planner_2_302.set_initial_value(1.0);
18657 cartesian_trajectory_planner_DW.obj_f.isSetupComplete = true;
18658
18659 // End of Start for MATLABSystem: '<S10>/Get Parameter3'
18660
18661 // Start for MATLABSystem: '<S10>/Get Parameter4'
18662 cartesian_trajectory_planner_DW.obj_e.matlabCodegenIsDeleted = true;
18663 cartesian_trajectory_planner_DW.obj_e.isInitialized = 0;
18664 cartesian_trajectory_planner_DW.obj_e.matlabCodegenIsDeleted = false;
18665 cartesian_trajectory_planner_DW.obj_e.isSetupComplete = false;
18666 cartesian_trajectory_planner_DW.obj_e.isInitialized = 1;
18667 for (cartesian_trajectory_planner__B.i1 = 0;
18668 cartesian_trajectory_planner__B.i1 < 10;
18669 cartesian_trajectory_planner__B.i1++) {
18670 cartesian_trajectory_planner__B.cv2[cartesian_trajectory_planner__B.i1] =
18671 tmp_j[cartesian_trajectory_planner__B.i1];
18672 }
18673
18674 cartesian_trajectory_planner__B.cv2[10] = '\x00';
18675 ParamGet_cartesian_trajectory_planner_2_303.initialize
18676 (cartesian_trajectory_planner__B.cv2);
18677 ParamGet_cartesian_trajectory_planner_2_303.initialize_error_codes(0, 1, 2,
18678 3);
18679 ParamGet_cartesian_trajectory_planner_2_303.set_initial_value(1.0);
18680 cartesian_trajectory_planner_DW.obj_e.isSetupComplete = true;
18681
18682 // End of Start for MATLABSystem: '<S10>/Get Parameter4'
18683
18684 // Start for MATLABSystem: '<S10>/Get Parameter5'
18685 cartesian_trajectory_planner_DW.obj_a.matlabCodegenIsDeleted = true;
18686 cartesian_trajectory_planner_DW.obj_a.isInitialized = 0;
18687 cartesian_trajectory_planner_DW.obj_a.matlabCodegenIsDeleted = false;
18688 cartesian_trajectory_planner_DW.obj_a.isSetupComplete = false;
18689 cartesian_trajectory_planner_DW.obj_a.isInitialized = 1;
18690 for (cartesian_trajectory_planner__B.i1 = 0;
18691 cartesian_trajectory_planner__B.i1 < 10;
18692 cartesian_trajectory_planner__B.i1++) {
18693 cartesian_trajectory_planner__B.cv2[cartesian_trajectory_planner__B.i1] =
18694 tmp_k[cartesian_trajectory_planner__B.i1];
18695 }
18696
18697 cartesian_trajectory_planner__B.cv2[10] = '\x00';
18698 ParamGet_cartesian_trajectory_planner_2_304.initialize
18699 (cartesian_trajectory_planner__B.cv2);
18700 ParamGet_cartesian_trajectory_planner_2_304.initialize_error_codes(0, 1, 2,
18701 3);
18702 ParamGet_cartesian_trajectory_planner_2_304.set_initial_value(0.0);
18703 cartesian_trajectory_planner_DW.obj_a.isSetupComplete = true;
18704
18705 // End of Start for MATLABSystem: '<S10>/Get Parameter5'
18706
18707 // Constant: '<Root>/Constant'
18708 cartesian_tra_MATLABSystem_Init
18709 (&cartesian_trajectory_planner__B.MATLABSystem,
18710 &cartesian_trajectory_planner_DW.MATLABSystem);
18711 cartesian_tra_MATLABSystem_Init
18712 (&cartesian_trajectory_planner__B.MATLABSystem_p,
18713 &cartesian_trajectory_planner_DW.MATLABSystem_p);
18714
18715 // Start for MATLABSystem: '<S9>/Get Parameter3'
18716 cartesian_trajectory_planner_DW.obj_k.matlabCodegenIsDeleted = true;
18717 cartesian_trajectory_planner_DW.obj_k.isInitialized = 0;
18718 cartesian_trajectory_planner_DW.obj_k.matlabCodegenIsDeleted = false;
18719 cartesian_trajectory_planner_DW.obj_k.isSetupComplete = false;
18720 cartesian_trajectory_planner_DW.obj_k.isInitialized = 1;
18721 for (cartesian_trajectory_planner__B.i1 = 0;
18722 cartesian_trajectory_planner__B.i1 < 12;
18723 cartesian_trajectory_planner__B.i1++) {
18724 cartesian_trajectory_planner__B.cv1[cartesian_trajectory_planner__B.i1] =
18725 tmp_l[cartesian_trajectory_planner__B.i1];
18726 }
18727
18728 cartesian_trajectory_planner__B.cv1[12] = '\x00';
18729 ParamGet_cartesian_trajectory_planner_2_272.initialize
18730 (cartesian_trajectory_planner__B.cv1);
18731 ParamGet_cartesian_trajectory_planner_2_272.initialize_error_codes(0, 1, 2,
18732 3);
18733 ParamGet_cartesian_trajectory_planner_2_272.set_initial_value(1.0);
18734 cartesian_trajectory_planner_DW.obj_k.isSetupComplete = true;
18735
18736 // End of Start for MATLABSystem: '<S9>/Get Parameter3'
18737
18738 // Start for MATLABSystem: '<S9>/Get Parameter2'
18739 cartesian_trajectory_planner_DW.obj_cu.matlabCodegenIsDeleted = true;
18740 cartesian_trajectory_planner_DW.obj_cu.isInitialized = 0;
18741 cartesian_trajectory_planner_DW.obj_cu.matlabCodegenIsDeleted = false;
18742 cartesian_trajectory_planner_DW.obj_cu.isSetupComplete = false;
18743 cartesian_trajectory_planner_DW.obj_cu.isInitialized = 1;
18744 for (cartesian_trajectory_planner__B.i1 = 0;
18745 cartesian_trajectory_planner__B.i1 < 12;
18746 cartesian_trajectory_planner__B.i1++) {
18747 cartesian_trajectory_planner__B.cv1[cartesian_trajectory_planner__B.i1] =
18748 tmp_m[cartesian_trajectory_planner__B.i1];
18749 }
18750
18751 cartesian_trajectory_planner__B.cv1[12] = '\x00';
18752 ParamGet_cartesian_trajectory_planner_2_271.initialize
18753 (cartesian_trajectory_planner__B.cv1);
18754 ParamGet_cartesian_trajectory_planner_2_271.initialize_error_codes(0, 1, 2,
18755 3);
18756 ParamGet_cartesian_trajectory_planner_2_271.set_initial_value(1.0);
18757 cartesian_trajectory_planner_DW.obj_cu.isSetupComplete = true;
18758
18759 // End of Start for MATLABSystem: '<S9>/Get Parameter2'
18760
18761 // Start for MATLABSystem: '<S9>/Get Parameter'
18762 cartesian_trajectory_planner_DW.obj_iy.matlabCodegenIsDeleted = true;
18763 cartesian_trajectory_planner_DW.obj_iy.isInitialized = 0;
18764 cartesian_trajectory_planner_DW.obj_iy.matlabCodegenIsDeleted = false;
18765 cartesian_trajectory_planner_DW.obj_iy.isSetupComplete = false;
18766 cartesian_trajectory_planner_DW.obj_iy.isInitialized = 1;
18767 for (cartesian_trajectory_planner__B.i1 = 0;
18768 cartesian_trajectory_planner__B.i1 < 12;
18769 cartesian_trajectory_planner__B.i1++) {
18770 cartesian_trajectory_planner__B.cv1[cartesian_trajectory_planner__B.i1] =
18771 tmp_n[cartesian_trajectory_planner__B.i1];
18772 }
18773
18774 cartesian_trajectory_planner__B.cv1[12] = '\x00';
18775 ParamGet_cartesian_trajectory_planner_2_270.initialize
18776 (cartesian_trajectory_planner__B.cv1);
18777 ParamGet_cartesian_trajectory_planner_2_270.initialize_error_codes(0, 1, 2,
18778 3);
18779 ParamGet_cartesian_trajectory_planner_2_270.set_initial_value(40.0);
18780 cartesian_trajectory_planner_DW.obj_iy.isSetupComplete = true;
18781
18782 // End of Start for MATLABSystem: '<S9>/Get Parameter'
18783
18784 // Start for MATLABSystem: '<Root>/Trapezoidal Velocity Profile Trajectory'
18785 cartesian_trajectory_planner_DW.obj.PeakVelocity[0] = 2.0;
18786 cartesian_trajectory_planner_DW.obj.Acceleration[0] = 1.0;
18787 cartesian_trajectory_planner_DW.obj.PeakVelocity[1] = 2.0;
18788 cartesian_trajectory_planner_DW.obj.Acceleration[1] = 2.0;
18789 cartesian_trajectory_planner_DW.obj.isInitialized = 0;
18790 cartesian_traj_SystemCore_setup(&cartesian_trajectory_planner_DW.obj);
18791 }
18792}
18793
18794// Model terminate function
18795void cartesian_trajectory_planner_2_terminate(void)
18796{
18797 // Terminate for MATLABSystem: '<S13>/Get Parameter3'
18798 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_o);
18799
18800 // Terminate for MATLABSystem: '<S13>/Get Parameter4'
18801 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_es);
18802
18803 // Terminate for MATLABSystem: '<S13>/Get Parameter5'
18804 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_f4);
18805
18806 // Terminate for MATLABSystem: '<S13>/Get Parameter6'
18807 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_en);
18808
18809 // Terminate for MATLABSystem: '<S13>/Get Parameter'
18810 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_b);
18811
18812 // Terminate for MATLABSystem: '<S13>/Get Parameter1'
18813 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_i);
18814
18815 // Terminate for MATLABSystem: '<S13>/Get Parameter2'
18816 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_db);
18817
18818 // Terminate for MATLABSystem: '<S12>/Get Parameter3'
18819 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_e0);
18820
18821 // Terminate for MATLABSystem: '<S12>/Get Parameter4'
18822 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_p);
18823
18824 // Terminate for MATLABSystem: '<S12>/Get Parameter5'
18825 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_g);
18826
18827 // Terminate for MATLABSystem: '<S12>/Get Parameter6'
18828 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_or);
18829
18830 // Terminate for MATLABSystem: '<S12>/Get Parameter'
18831 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_a3);
18832
18833 // Terminate for MATLABSystem: '<S12>/Get Parameter1'
18834 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_bh);
18835
18836 // Terminate for MATLABSystem: '<S12>/Get Parameter2'
18837 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_b0);
18838
18839 // Terminate for MATLABSystem: '<S10>/Get Parameter'
18840 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_d);
18841
18842 // Terminate for MATLABSystem: '<S10>/Get Parameter1'
18843 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_m);
18844
18845 // Terminate for MATLABSystem: '<S10>/Get Parameter2'
18846 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_c);
18847
18848 // Terminate for MATLABSystem: '<S10>/Get Parameter3'
18849 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_f);
18850
18851 // Terminate for MATLABSystem: '<S10>/Get Parameter4'
18852 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_e);
18853
18854 // Terminate for MATLABSystem: '<S10>/Get Parameter5'
18855 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_a);
18856 cartesian_tra_MATLABSystem_Term(&cartesian_trajectory_planner_DW.MATLABSystem);
18857 cartesian_tra_MATLABSystem_Term
18858 (&cartesian_trajectory_planner_DW.MATLABSystem_p);
18859
18860 // Terminate for MATLABSystem: '<S9>/Get Parameter3'
18861 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_k);
18862
18863 // Terminate for MATLABSystem: '<S9>/Get Parameter2'
18864 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_cu);
18865
18866 // Terminate for Atomic SubSystem: '<Root>/Subscribe'
18867 // Terminate for MATLABSystem: '<S8>/SourceBlock'
18868 matlabCodegenHandle_matlabCo_jb(&cartesian_trajectory_planner_DW.obj_cs);
18869
18870 // End of Terminate for SubSystem: '<Root>/Subscribe'
18871
18872 // Terminate for MATLABSystem: '<S9>/Get Parameter'
18873 matlabCodegenHandle_matlabC_jbz(&cartesian_trajectory_planner_DW.obj_iy);
18874
18875 // Terminate for Atomic SubSystem: '<Root>/Publish'
18876 // Terminate for MATLABSystem: '<S7>/SinkBlock'
18877 matlabCodegenHandle_matlabCod_j(&cartesian_trajectory_planner_DW.obj_ej);
18878
18879 // End of Terminate for SubSystem: '<Root>/Publish'
18880}
18881
18882//
18883// File trailer for generated code.
18884//
18885// [EOF]
18886//
18887